Skip to content

P
proview
Commit 194563c4 authored by claes's avatar claes
Browse files
Options

*** empty log message ***

parent 0a9ee57b
Hide whitespace changes
Inline Side-by-side
Showing with 733 additions and 228 deletions
src/lib/rt/src/os_linux/rt_io_m_pb_ai.c
View file @ 194563c4
......@@ -31,10 +31,11 @@
void ConvertAi (
pwr_sClass_ChanAi *cop,
pwr_tInt16 nobits,
pwr_tInt16 rawvalue16,
pwr_tInt32 rawvalue32,
pwr_tUInt8 rawvalue8,
pwr_tUInt16 rawvalue16,
pwr_tUInt32 rawvalue32,
pwr_tFloat32 *actvalue_p,
io_sCardLocal *local
pwr_tEnum representation
)
{
......@@ -45,17 +46,23 @@ void ConvertAi (
int i;
if (nobits == 32) {
if (local->unsign_rawval)
f_raw = (float) ((unsigned int) rawvalue32);
if (representation == PB_NUMREP_SIGNEDINT)
f_raw = (float) ((int) rawvalue32);
else
f_raw = (float) rawvalue32;
}
else {
if (local->unsign_rawval)
f_raw = (float) ((unsigned short) rawvalue16);
else if (nobits == 16){
if (representation == PB_NUMREP_SIGNEDINT)
f_raw = (float) ((short) rawvalue16);
else
f_raw = (float) rawvalue16;
}
else if (nobits == 8){
if (representation == PB_NUMREP_SIGNEDINT)
f_raw = (float) ((char) rawvalue8);
else
f_raw = (float) rawvalue8;
}
switch ( cop->SensorPolyType)
{
......@@ -102,10 +109,11 @@ void ConvertAi (
void ConvertAit (
pwr_sClass_ChanAit *cop,
pwr_tInt16 nobits,
pwr_tInt16 rawvalue16,
pwr_tInt32 rawvalue32,
pwr_tUInt16 rawvalue8,
pwr_tUInt16 rawvalue16,
pwr_tUInt32 rawvalue32,
pwr_tFloat32 *actvalue_p,
io_sCardLocal *local
pwr_tEnum representation
)
{
pwr_tFloat32 Slope;
......@@ -118,17 +126,23 @@ void ConvertAit (
pwr_tFloat32 f_raw;
if (nobits == 32) {
if (local->unsign_rawval)
f_raw = (float) ((unsigned int) rawvalue32);
if (representation == PB_NUMREP_SIGNEDINT)
f_raw = (float) ((int) rawvalue32);
else
f_raw = (float) rawvalue32;
}
else {
if (local->unsign_rawval)
f_raw = (float) ((unsigned short) rawvalue16);
else if (nobits == 16) {
if (representation == PB_NUMREP_SIGNEDINT)
f_raw = (float) ((short) rawvalue16);
else
f_raw = (float) rawvalue16;
}
else if (nobits == 8) {
if (representation == PB_NUMREP_SIGNEDINT)
f_raw = (float) ((char) rawvalue8);
else
f_raw = (float) rawvalue8;
}
sigvalue = cop->SigValPolyCoef0 + cop->SigValPolyCoef1 * f_raw;
......@@ -163,24 +177,11 @@ static pwr_tStatus IoCardInit (
pwr_sClass_Pb_Ai *op;
int i;
io_sChannel *chanp;
pwr_sClass_Pb_DP_Slave *slave;
op = (pwr_sClass_Pb_Ai *) cp->op;
local = (io_sCardLocal *) cp->Local;
local->byte_swap = 0;
local->unsign_rawval = 0;
if (rp->Class == pwr_cClass_Pb_DP_Slave) {
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
/* Byte swap if Big Endian (bit 0) */
if (slave->ByteOrdering & 1) local->byte_swap = 1;
/* Signed or unsigned rawvalue? (bit 2) */
if (slave->ByteOrdering & 1<<2) local->unsign_rawval = 1;
}
else {
if (rp->Class != pwr_cClass_Pb_DP_Slave) {
errh_Info( "Illegal object type %s", cp->Name );
return 1;
}
......@@ -216,9 +217,12 @@ static pwr_tStatus IoCardRead (
{
io_sCardLocal *local;
pwr_sClass_Pb_Ai *op;
pwr_sClass_Pb_DP_Slave *slave;
io_sChannel *chanp;
pwr_sClass_ChanAi *cop;
pwr_sClass_Ai *sop;
pwr_tInt8 data8 = 0;
pwr_tUInt8 udata8 = 0;
pwr_tInt16 data16 = 0;
pwr_tUInt16 udata16 = 0;
pwr_tInt32 data32 = 0;
......@@ -228,7 +232,8 @@ static pwr_tStatus IoCardRead (
local = (io_sCardLocal *) cp->Local;
op = (pwr_sClass_Pb_Ai *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
for (i=0; i<cp->ChanListSize; i++) {
......@@ -243,39 +248,58 @@ static pwr_tStatus IoCardRead (
if (cop->ConversionOn) {
if (local->scancount[i] <= 1) {
if (op->BytesPerChannel == 4) {
memcpy(&udata32, local->input_area + op->OffsetInputs + 4*i, 4);
if (local->byte_swap == 1) udata32 = swap32(udata32);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) udata32 = swap32(udata32);
data32 = (pwr_tInt32) udata32;
sop->RawValue = 0;
if (local->unsign_rawval)
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT)
sop->SigValue = udata32 * cop->SigValPolyCoef1 + cop->SigValPolyCoef0;
else
sop->SigValue = data32 * cop->SigValPolyCoef1 + cop->SigValPolyCoef0;
switch(chanp->ChanClass) {
case pwr_cClass_ChanAi:
ConvertAi(cop, 32, 0, data32, &actvalue, local);
ConvertAi(cop, 32, 0, 0, udata32, &actvalue, op->NumberRepresentation);
break;
case pwr_cClass_ChanAit:
ConvertAit((pwr_sClass_ChanAit *) cop, 32, 0, data32, &actvalue, local);
ConvertAit((pwr_sClass_ChanAit *) cop, 32, 0, 0, udata32, &actvalue, op->NumberRepresentation);
break;
}
}
else {
else if (op->BytesPerChannel == 2) {
memcpy(&udata16, local->input_area + op->OffsetInputs + 2*i, 2);
if (local->byte_swap == 1) udata16 = swap16(udata16);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) udata16 = swap16(udata16);
data16 = (pwr_tInt16) udata16;
sop->RawValue = udata16;
if (local->unsign_rawval)
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT)
sop->SigValue = udata16 * cop->SigValPolyCoef1 + cop->SigValPolyCoef0;
else
sop->SigValue = data16 * cop->SigValPolyCoef1 + cop->SigValPolyCoef0;
switch(chanp->ChanClass) {
case pwr_cClass_ChanAi:
ConvertAi(cop, 16, data16, 0, &actvalue, local);
ConvertAi(cop, 16, 0, udata16, 0, &actvalue, op->NumberRepresentation);
break;
case pwr_cClass_ChanAit:
ConvertAit((pwr_sClass_ChanAit *) cop, 16, 0, udata16, 0, &actvalue, op->NumberRepresentation);
break;
}
}
else if (op->BytesPerChannel == 1) {
memcpy(&udata8, local->input_area + op->OffsetInputs + i, 1);
data8 = (pwr_tInt8) udata8;
sop->RawValue = udata8;
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT)
sop->SigValue = udata8 * cop->SigValPolyCoef1 + cop->SigValPolyCoef0;
else
sop->SigValue = data8 * cop->SigValPolyCoef1 + cop->SigValPolyCoef0;
switch(chanp->ChanClass) {
case pwr_cClass_ChanAi:
ConvertAi(cop, 8, udata8, 0, 0, &actvalue, op->NumberRepresentation);
break;
case pwr_cClass_ChanAit:
ConvertAit((pwr_sClass_ChanAit *) cop, 16, data16, 0, &actvalue, local);
ConvertAit((pwr_sClass_ChanAit *) cop, 8, udata8, 0, 0, &actvalue, op->NumberRepresentation);
break;
}
}
......
src/lib/rt/src/os_linux/rt_io_m_pb_ao.c
View file @ 194563c4
......@@ -36,20 +36,12 @@ static pwr_tStatus IoCardInit (
io_sCardLocal *local;
pwr_sClass_Pb_Ao *op;
io_sChannel *chanp;
pwr_sClass_Pb_DP_Slave *slave;
int i;
op = (pwr_sClass_Pb_Ao *) cp->op;
local = (io_sCardLocal *) cp->Local;
if (rp->Class == pwr_cClass_Pb_DP_Slave) {
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
/* Byte swap if Big Endian (bit 0) */
if (slave->ByteOrdering & 1) local->byte_swap = 1;
}
else {
if (rp->Class != pwr_cClass_Pb_DP_Slave) {
errh_Info( "Illegal object type %s", cp->Name );
return 1;
}
......@@ -82,7 +74,9 @@ static pwr_tStatus IoCardWrite (
{
io_sCardLocal *local;
pwr_sClass_Pb_Ao *op;
pwr_sClass_Pb_DP_Slave *slave;
int i;
pwr_tInt8 data8 = 0;
pwr_tInt16 data16 = 0;
pwr_tInt32 data32 = 0;
pwr_sClass_ChanAo *cop;
......@@ -94,6 +88,7 @@ static pwr_tStatus IoCardWrite (
local = (io_sCardLocal *) cp->Local;
op = (pwr_sClass_Pb_Ao *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
......@@ -137,21 +132,31 @@ static pwr_tStatus IoCardWrite (
sop->RawValue = 0;
data32 = (pwr_tInt32) rawvalue;
if (local->byte_swap == 1) data32 = swap32(data32);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data32 = swap32(data32);
memcpy(local->output_area + op->OffsetOutputs + 4*i, &data32, 4);
}
else {
else if (op->BytesPerChannel == 2) {
if ( rawvalue > 0)
sop->RawValue = rawvalue + 0.5;
else
sop->RawValue = rawvalue - 0.5;
data16 = (pwr_tInt16) sop->RawValue;
if (local->byte_swap == 1) data16 = swap16(data16);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data16 = swap16(data16);
memcpy(local->output_area + op->OffsetOutputs + 2*i, &data16, 2);
}
else if (op->BytesPerChannel == 1) {
if ( rawvalue > 0)
sop->RawValue = rawvalue + 0.5;
else
sop->RawValue = rawvalue - 0.5;
data8 = (pwr_tInt8) sop->RawValue;
memcpy(local->output_area + op->OffsetOutputs + i, &data8, 1);
}
}
}
......
src/lib/rt/src/os_linux/rt_io_m_pb_di.c
View file @ 194563c4
......@@ -35,32 +35,19 @@ static pwr_tStatus IoCardInit (
{
io_sCardLocal *local;
pwr_sClass_Pb_Di *op;
pwr_sClass_Pb_DP_Slave *slave;
op = (pwr_sClass_Pb_Di *) cp->op;
local = (io_sCardLocal *) cp->Local;
local->byte_swap = 0;
if (rp->Class == pwr_cClass_Pb_DP_Slave) {
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
/* Byte swap if Big Endian (bit 0) and
Digital modules wordoriented (bit 1) */
if ((slave->ByteOrdering & 1) &&
(slave->ByteOrdering & 1<<1))
local->byte_swap = 1;
}
else {
errh_Info( "Illegal object type %s", cp->Name );
return 1;
}
// Check configuration
if (op->NumberOfChannels != 8 && op->NumberOfChannels != 16 && op->NumberOfChannels != 32)
op->Status = 0;
if (op->Orientation > op->NumberOfChannels)
op->Status = 0;
if (op->Status < 1) {
errh_Info( "Error initializing Pb module Di %s", cp->Name );
}
if (op->Status < 1) errh_Info( "Error initializing Pb module Di %s", cp->Name );
return 1;
}
......@@ -78,35 +65,40 @@ static pwr_tStatus IoCardRead (
{
io_sCardLocal *local;
pwr_sClass_Pb_Di *op;
pwr_tUInt16 data[2] = {0, 0};
pwr_tUInt16 invmask;
pwr_tUInt16 convmask;
int i;
pwr_sClass_Pb_DP_Slave *slave;
pwr_tUInt16 data[2] = {0, 0};
pwr_tUInt32 *data32;
local = (io_sCardLocal *) cp->Local;
op = (pwr_sClass_Pb_Di *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
memcpy(&data, local->input_area + op->OffsetInputs, op->BytesOfInput);
for (i=0; i<2; i++) {
if (i==0) {
convmask = op->ConvMask1;
invmask = op->InvMask1;
if (slave->ByteOrdering == PB_BYTEORDERING_BE) {
if (op->Orientation == PB_ORIENTATION_WORD) {
data[0] = swap16(data[0]);
data[1] = swap16(data[1]);
}
else {
convmask = op->ConvMask2;
invmask = op->InvMask2;
if (op->NumberOfChannels <= 16) break;
else if (op->Orientation == PB_ORIENTATION_DWORD) {
data32 = (pwr_tUInt32 *) &data;
*data32 = swap32(*data32);
}
}
if (local->byte_swap == 1) data[i] = swap16(data[i]);
data[i] = data[i] ^ invmask;
io_DiUnpackWord(cp, data[i], convmask, i);
// Packa upp
data[0] = data[0] ^ op->InvMask1;
io_DiUnpackWord(cp, data[0], op->ConvMask1, 0);
if (op->NumberOfChannels > 16) {
data[1] = data[1] ^ op->InvMask2;
io_DiUnpackWord(cp, data[1], op->ConvMask2, 1);
}
}
return 1;
}
......
src/lib/rt/src/os_linux/rt_io_m_pb_do.c
View file @ 194563c4
......@@ -35,32 +35,17 @@ static pwr_tStatus IoCardInit (
{
io_sCardLocal *local;
pwr_sClass_Pb_Do *op;
pwr_sClass_Pb_DP_Slave *slave;
op = (pwr_sClass_Pb_Do *) cp->op;
local = (io_sCardLocal *) cp->Local;
local->byte_swap = 0;
if (rp->Class == pwr_cClass_Pb_DP_Slave) {
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
/* Byte swap if Big Endian (bit 0) and
Digital modules wordoriented (bit 1) */
if ((slave->ByteOrdering & 1) &&
(slave->ByteOrdering & 1<<1))
local->byte_swap = 1;
}
else {
errh_Info( "Error initializing Pb module Do %s", cp->Name );
return 1;
}
if (op->NumberOfChannels != 8 && op->NumberOfChannels != 16 && op->NumberOfChannels != 32)
op->Status = 0;
if (op->Orientation > op->NumberOfChannels)
op->Status = 0;
if (op->Status < 1) {
errh_Info( "Error initializing Pb module Do %s", cp->Name );
}
if (op->Status < 1) errh_Info( "Error initializing Pb module Do %s", cp->Name );
return 1;
}
......@@ -78,21 +63,31 @@ static pwr_tStatus IoCardWrite (
{
io_sCardLocal *local;
pwr_sClass_Pb_Do *op;
pwr_sClass_Pb_DP_Slave *slave;
pwr_tUInt16 data[2] = {0, 0};
// pwr_tUInt16 invmask;
// pwr_tUInt16 convmask;
int i;
pwr_tUInt32 *data32;
local = (io_sCardLocal *) cp->Local;
op = (pwr_sClass_Pb_Do *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
for (i=0; i<2; i++) {
if (i==1 && op->NumberOfChannels <= 16) break;
io_DoPackWord(cp, &data[i], i);
if (local->byte_swap == 1) data[i] = swap16(data[i]);
}
io_DoPackWord(cp, &data[0], 0);
if (op->NumberOfChannels > 16) io_DoPackWord(cp, &data[1], 1);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) {
if (op->Orientation == PB_ORIENTATION_WORD) {
data[0] = swap16(data[0]);
data[1] = swap16(data[1]);
}
else if (op->Orientation == PB_ORIENTATION_DWORD) {
data32 = (pwr_tUInt32 *) &data;
*data32 = swap32(*data32);
}
}
memcpy(local->output_area + op->OffsetOutputs, &data, op->BytesOfOutput);
}
return 1;
......
src/lib/rt/src/os_linux/rt_io_m_pb_dp_slave.c
View file @ 194563c4
......@@ -35,8 +35,9 @@ static pwr_tStatus IoRackInit (
io_sRack *rp
)
{
/*
io_sAgentLocal *local_agent;
io_sRackLocal *local_rack;
io_sCardLocal *local_card;
pwr_sClass_Pb_DP_Slave *op;
......@@ -52,9 +53,10 @@ static pwr_tStatus IoRackInit (
pwr_sClass_Pb_Do *dop;
pwr_sClass_Pb_Ai *aip;
pwr_sClass_Pb_Ao *aop;
pwr_sClass_Pb_Ii *iip;
pwr_sClass_Pb_Io *iop;
local_agent = (io_sAgentLocal *) (ap->Local);
local_rack = (io_sRackLocal *) (rp->Local);
fp = local_agent->Pb_fp;
......@@ -65,9 +67,6 @@ static pwr_tStatus IoRackInit (
// Try to initialize slave.
local_rack->initialized = 0;
op->Status = 0;
if (op->Status < 1) {
// Three attempts
......@@ -120,12 +119,18 @@ static pwr_tStatus IoRackInit (
op->NumberModules = 0;
while(cardp) {
local_card = calloc(1, sizeof(*local_card));
cardp->Local = local_card;
if (!cardp->Local) {
local_card = calloc(1, sizeof(*local_card));
cardp->Local = local_card;
}
else
local_card = cardp->Local;
local_card->input_area = (void *) &(op->Inputs);
local_card->output_area = (void *) &(op->Outputs);
local_card->initialized = 0;
errh_Info("Init module %s", cardp->Name);
switch (cardp->Class) {
case pwr_cClass_Pb_Di:
......@@ -163,6 +168,24 @@ static pwr_tStatus IoRackInit (
aop->Status = 1;
local_card->initialized = 1;
break;
case pwr_cClass_Pb_Ii:
iip = (pwr_sClass_Pb_Ii *) cardp->op;
iip->OffsetInputs = input_counter;
iip->BytesOfInput = iip->NumberOfChannels * iip->BytesPerChannel;
input_counter += iip->BytesOfInput;
iip->Status = 1;
local_card->initialized = 1;
break;
case pwr_cClass_Pb_Io:
iop = (pwr_sClass_Pb_Io *) cardp->op;
iop->OffsetOutputs = output_counter;
iop->BytesOfOutput = iop->NumberOfChannels * iop->BytesPerChannel;
output_counter += iop->BytesOfOutput;
iop->Status = 1;
local_card->initialized = 1;
break;
}
op->NumberModules++;
......@@ -175,9 +198,10 @@ static pwr_tStatus IoRackInit (
// return IO__SUCCESS;
}
op->Status = 1;
local_rack->initialized = 1;
op->Status = PB_SLAVE_STATE_STOPPED;
*/
errh_Info( "Init DP slave %s", rp->Name );
return IO__SUCCESS;
}
......@@ -191,39 +215,33 @@ static pwr_tStatus IoRackRead (
io_sRack *rp
)
{
io_sRackLocal *local;
io_sAgentLocal *agent_local;
pwr_sClass_Pb_DP_Slave *op;
int fp;
unsigned char diag;
pwr_tUInt16 sts;
local = (io_sRackLocal *) rp->Local;
agent_local = (io_sAgentLocal *) (ap->Local);
fp = agent_local->Pb_fp;
op = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (local->initialized == 1)
op->Status = 1;
else
op->Status = 0;
if (op->Status > 0 && op->DisableSlave == 0) {
if (op->Status > PB_SLAVE_STATE_NOTINIT && op->DisableSlave == 0) {
sts = pb_cmi_get_data(fp, ID_DP_STATUS_IMAGE, op->SlaveAddress, 1, &diag);
if ((sts != PB_OK) || (diag & 1)) {
op->Status = 1;
op->Status = PB_SLAVE_STATE_STOPPED;
op->ErrorCount++;
if (op->StallAction > 0) {
if (op->ErrorCount > op->ErrorSoftLimit && op->StallAction >= PB_STALLACTION_RESET) {
memset(&op->Inputs, 0, op->BytesOfInput);
}
}
else {
op->Status = 2;
op->Status = PB_SLAVE_STATE_OPERATE;
}
if ((op->Status > 1) && op->BytesOfInput > 0) {
if ((op->Status > PB_SLAVE_STATE_STOPPED) && op->BytesOfInput > 0) {
sts = pb_cmi_get_data(fp,
ID_DP_SLAVE_IO_IMAGE,
......@@ -237,10 +255,13 @@ static pwr_tStatus IoRackRead (
op->ErrorCount = 0;
}
if (op->ErrorCount > op->ErrorHardLimit && op->StallAction == 2)
if (op->ErrorCount > op->ErrorHardLimit && op->StallAction >= PB_STALLACTION_BREAK)
ctx->Node->EmergBreakTrue = 1;
}
/*
else if (op->Status == PB_SLAVE_STATE_NOTINIT)
IoRackInit(ctx, ap, rp);
*/
return IO__SUCCESS;
}
......@@ -254,20 +275,18 @@ static pwr_tStatus IoRackWrite (
io_sRack *rp
)
{
io_sRackLocal *local;
io_sAgentLocal *agent_local;
pwr_sClass_Pb_DP_Slave *op;
int fp;
pwr_tUInt16 sts;
local = (io_sRackLocal *) rp->Local;
agent_local = (io_sAgentLocal *) (ap->Local);
fp = agent_local->Pb_fp;
op = (pwr_sClass_Pb_DP_Slave *) rp->op;
// Write the whole I/O output area from local area
if (op->Status > 0 && op->DisableSlave == 0) {
if (op->Status > PB_SLAVE_STATE_NOTINIT && op->DisableSlave == 0) {
if (op->BytesOfOutput > 0) {
......@@ -294,13 +313,6 @@ static pwr_tStatus IoRackClose (
io_sRack *rp
)
{
io_sRackLocal *local;
/* Free dynamic memory */
local = rp->Local;
free( (char *)local);
return 1;
}
......
src/lib/rt/src/os_linux/rt_io_m_pb_ii.c 0 → 100644
View file @ 194563c4
/* rt_io_m_pb_ii.c
PROVIEW/R */
#pragma pack(1)
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <math.h>
#include <sys/file.h>
#include <sys/ioctl.h>
#include "pb_type.h"
#include "pb_if.h"
#include "pb_fmb.h"
#include "pb_dp.h"
#include "rt_io_pb_locals.h"
#include "pwr.h"
#include "pwr_baseclasses.h"
#include "rt_io_base.h"
#include "rt_io_msg.h"
#include "rt_errh.h"
#include "rt_io_profiboard.h"
/*----------------------------------------------------------------------------*\
Init method for the Pb module Ii
\*----------------------------------------------------------------------------*/
static pwr_tStatus IoCardInit (
io_tCtx ctx,
io_sAgent *ap,
io_sRack *rp,
io_sCard *cp
)
{
io_sCardLocal *local;
pwr_sClass_Pb_Ii *op;
op = (pwr_sClass_Pb_Ii *) cp->op;
local = (io_sCardLocal *) cp->Local;
if (rp->Class != pwr_cClass_Pb_DP_Slave) {
errh_Info( "Illegal object type %s", cp->Name );
return 1;
}
if (op->Status < 1) errh_Info( "Error initializing Pb module Ii %s", cp->Name );
return 1;
}
/*----------------------------------------------------------------------------*\
Read method for the Pb Ii card
\*----------------------------------------------------------------------------*/
static pwr_tStatus IoCardRead (
io_tCtx ctx,
io_sAgent *ap,
io_sRack *rp,
io_sCard *cp
)
{
io_sCardLocal *local;
pwr_sClass_Pb_Ii *op;
pwr_sClass_Pb_DP_Slave *slave;
io_sChannel *chanp;
pwr_sClass_ChanIi *cop;
pwr_sClass_Ii *sop;
pwr_tUInt8 udata8 = 0;
pwr_tUInt16 udata16 = 0;
pwr_tUInt32 udata32 = 0;
pwr_tInt8 data8 = 0;
pwr_tInt16 data16 = 0;
pwr_tInt32 data32 = 0;
int i;
local = (io_sCardLocal *) cp->Local;
op = (pwr_sClass_Pb_Ii *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
for (i=0; i<cp->ChanListSize; i++) {
chanp = &cp->chanlist[i];
if (!chanp->cop) continue;
cop = (pwr_sClass_ChanIi *) chanp->cop;
sop = (pwr_sClass_Ii *) chanp->sop;
if (cop->ConversionOn) {
if (op->BytesPerChannel == 4) {
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT) {
memcpy(&udata32, local->input_area + op->OffsetInputs + 4*i, 4);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) udata32 = swap32(udata32);
*(pwr_tInt32 *) chanp->vbp = (pwr_tInt32) udata32;
}
else if (op->NumberRepresentation == PB_NUMREP_SIGNEDINT) {
memcpy(&data32, local->input_area + op->OffsetInputs + 4*i, 4);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data32 = swap32(data32);
*(pwr_tInt32 *) chanp->vbp = data32;
}
}
else if (op->BytesPerChannel == 2) {
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT) {
memcpy(&udata16, local->input_area + op->OffsetInputs + 2*i, 2);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) udata16 = swap16(udata16);
*(pwr_tInt32 *) chanp->vbp = (pwr_tInt32) udata16;
}
else if (op->NumberRepresentation == PB_NUMREP_SIGNEDINT) {
memcpy(&data16, local->input_area + op->OffsetInputs + 2*i, 2);
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data16 = swap16(data16);
*(pwr_tInt32 *) chanp->vbp = (pwr_tInt32) data16;
}
}
else if (op->BytesPerChannel == 1) {
if (op->NumberRepresentation == PB_NUMREP_UNSIGNEDINT) {
memcpy(&udata8, local->input_area + op->OffsetInputs + i, 1);
*(pwr_tInt32 *) chanp->vbp = (pwr_tInt32) udata8;
}
else if (op->NumberRepresentation == PB_NUMREP_SIGNEDINT) {
memcpy(&data8, local->input_area + op->OffsetInputs + i, 1);
*(pwr_tInt32 *) chanp->vbp = (pwr_tInt32) data8;
}
}
}
}
}
return 1;
}
/*----------------------------------------------------------------------------*\
Close method for the Pb Ii card
\*----------------------------------------------------------------------------*/
static pwr_tStatus IoCardClose (
io_tCtx ctx,
io_sAgent *ap,
io_sRack *rp,
io_sCard *cp
)
{
io_sCardLocal *local;
local = rp->Local;
free ((char *) local);
return 1;
}
/*----------------------------------------------------------------------------*\
Every method to be exported to the workbench should be registred here.
\*----------------------------------------------------------------------------*/
pwr_dExport pwr_BindIoMethods(Pb_Ii) = {
pwr_BindIoMethod(IoCardInit),
pwr_BindIoMethod(IoCardRead),
pwr_BindIoMethod(IoCardClose),
pwr_NullMethod
};
src/lib/rt/src/os_linux/rt_io_m_pb_io.c 0 → 100644
View file @ 194563c4
/* rt_io_m_pb_io.c
PROVIEW/R */
#pragma pack(1)
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <sys/file.h>
#include <sys/ioctl.h>
#include "pb_type.h"
#include "pb_if.h"
#include "pb_fmb.h"
#include "pb_dp.h"
#include "rt_io_pb_locals.h"
#include "pwr.h"
#include "pwr_baseclasses.h"
#include "rt_io_base.h"
#include "rt_io_msg.h"
#include "rt_errh.h"
#include "rt_io_profiboard.h"
/*----------------------------------------------------------------------------*\
Init method for the Pb module Io
\*----------------------------------------------------------------------------*/
static pwr_tStatus IoCardInit (
io_tCtx ctx,
io_sAgent *ap,
io_sRack *rp,
io_sCard *cp
)
{
io_sCardLocal *local;
pwr_sClass_Pb_Io *op;
op = (pwr_sClass_Pb_Io *) cp->op;
local = (io_sCardLocal *) cp->Local;
if (rp->Class != pwr_cClass_Pb_DP_Slave) {
errh_Info( "Illegal object type %s", cp->Name );
return 1;
}
if (op->Status < 1) {
errh_Info( "Error initializing Pb module Io %s", cp->Name );
}
return 1;
}
/*----------------------------------------------------------------------------*\
Write method for the Pb module Io
\*----------------------------------------------------------------------------*/
static pwr_tStatus IoCardWrite (
io_tCtx ctx,
io_sAgent *ap,
io_sRack *rp,
io_sCard *cp
)
{
io_sCardLocal *local;
pwr_sClass_Pb_Io *op;
pwr_sClass_Pb_DP_Slave *slave;
int i;
pwr_tInt8 data8 = 0;
pwr_tInt16 data16 = 0;
pwr_tInt32 data32 = 0;
pwr_sClass_ChanIo *cop;
pwr_sClass_Io *sop;
io_sChannel *chanp;
local = (io_sCardLocal *) cp->Local;
op = (pwr_sClass_Pb_Io *) cp->op;
slave = (pwr_sClass_Pb_DP_Slave *) rp->op;
if (op->Status >= 1) {
for (i=0; i<cp->ChanListSize; i++) {
chanp = &cp->chanlist[i];
if (!chanp->cop) continue;
cop = (pwr_sClass_ChanIo *) chanp->cop;
sop = (pwr_sClass_Io *) chanp->sop;
if (cop->TestOn != 0) continue;
data32 = *(pwr_tInt32 *) chanp->vbp;
if (op->BytesPerChannel == 4) {
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data32 = swap32(data32);
memcpy(local->output_area + op->OffsetOutputs + 4*i, &data32, 4);
}
else if (op->BytesPerChannel == 2) {
data16 = (pwr_tInt16) data32;
if (slave->ByteOrdering == PB_BYTEORDERING_BE) data16 = swap16(data16);
memcpy(local->output_area + op->OffsetOutputs + 2*i, &data16, 2);
}
else if (op->BytesPerChannel == 1) {
data8 = (pwr_tInt8) data32;
memcpy(local->output_area + op->OffsetOutputs + i, &data8, 1);
}
}
}
return 1;
}
/*----------------------------------------------------------------------------*\
\*----------------------------------------------------------------------------*/
static pwr_tStatus IoCardClose (
io_tCtx ctx,
io_sAgent *ap,
io_sRack *rp,
io_sCard *cp
)
{
io_sCardLocal *local;
local = rp->Local;
free ((char *) local);
return 1;
}
/*----------------------------------------------------------------------------*\
Every method to be exported to the workbench should be registred here.
\*----------------------------------------------------------------------------*/
pwr_dExport pwr_BindIoMethods(Pb_Io) = {
pwr_BindIoMethod(IoCardInit),
pwr_BindIoMethod(IoCardWrite),
pwr_BindIoMethod(IoCardClose),
pwr_NullMethod
};
src/lib/rt/src/os_linux/rt_io_m_pb_profiboard.c
View file @ 194563c4
......@@ -19,6 +19,7 @@
#include "rt_io_pb_locals.h"
#include "pwr.h"
#include "co_cdh.h"
#include "pwr_baseclasses.h"
#include "rt_io_base.h"
#include "rt_io_msg.h"
......@@ -27,6 +28,23 @@
#include "rt_io_profiboard.h"
static pwr_tStatus IoAgentInit (
io_tCtx ctx,
io_sAgent *ap
);
static pwr_tStatus IoAgentRead (
io_tCtx ctx,
io_sAgent *ap
);
static pwr_tStatus IoAgentWrite (
io_tCtx ctx,
io_sAgent *ap
);
static pwr_tStatus IoAgentClose (
io_tCtx ctx,
io_sAgent *ap
);
/*----------------------------------------------------------------------------*\
Sends request to Profiboard for setting FMB parameters
......@@ -223,6 +241,79 @@ static short act_param_loc(int fp,
return retval;
}
/*----------------------------------------------------------------------------*\
Initializes one DP slave in the master card
\*----------------------------------------------------------------------------*/
static pwr_tStatus init_dp_slave (
io_sAgent *ap,
pwr_tObjid oid
)
{
pwr_sClass_Pb_DP_Slave *op;
io_sAgentLocal *local_agent;
int i;
char name[196];
pwr_tUInt16 sts;
pwr_tStatus status;
struct timespec rqtp, rmtp;
status = gdh_ObjidToPointer(oid, (pwr_tAddress *) &op);
status = gdh_ObjidToName(oid, (char *) &name, sizeof(name), cdh_mNName);
local_agent = (io_sAgentLocal *) (ap->Local);
op->Status = PB_SLAVE_STATE_NOTINIT;
errh_Info( "Config of Profibus DP slave %s", name );
// Try to initialize slave, make three attempts before we give up
for(i=0; i<3; i++) {
if (op->AutoConfigure == 1) {
sts = pb_get_slave_cfg(local_agent->Pb_fp,
op->SlaveAddress,
&op->ConfigDataLen,
&op->ConfigData);
}
else {
sts = PB_OK;
}
if (sts == PB_OK) {
sts = pb_download_all(local_agent->Pb_fp,
op->SlaveAddress,
op->WdFact1,
op->WdFact2,
0,
op->PNOIdent,
op->GroupIdent,
op->PrmUserDataLen,
&op->PrmUserData,
op->ConfigDataLen,
&op->ConfigData);
}
if (sts == PB_OK) {
sts = pb_get_slave_info(local_agent->Pb_fp,
op->SlaveAddress,
&op->OffsetInputs,
&op->OffsetOutputs,
&op->BytesOfInput,
&op->BytesOfOutput);
}
if (sts == PB_OK) break;
rqtp.tv_sec = 1;
rqtp.tv_nsec = 0;
nanosleep(&rqtp, &rmtp);
}
if (sts != PB_OK) {
errh_Info( "ERROR Init Profibus DP slave %s", name);
return IO__ERRINIDEVICE;
}
op->Status = PB_SLAVE_STATE_STOPPED;
return IO__SUCCESS;
}
/*----------------------------------------------------------------------------*\
Init method for the Pb_profiboard agent
......@@ -232,78 +323,121 @@ static pwr_tStatus IoAgentInit (
io_sAgent *ap
)
{
io_sAgentLocal *local_master;
pwr_sClass_Pb_Profiboard *op;
pwr_tUInt16 sts;
io_sRack *slavep;
io_sRackLocal *local_slave;
pwr_tStatus status;
io_sAgentLocal *local;
unsigned char devname[25];
/* Allocate area for local data structure */
local_master = calloc(1, sizeof(*local_master));
ap->Local = local_master;
pwr_tObjid slave_objid;
pwr_tClassId slave_class;
op = (pwr_sClass_Pb_Profiboard *) ap->op;
/* Open Pb driver */
sprintf(devname, "/dev/pbus%1d", op->BusNumber);
local_master->Pb_fp = open(devname, O_RDWR);
errh_Info( "Config of Profibus DP Master %s", ap->Name );
if ( local_master->Pb_fp == -1)
{
/* Can't open driver */
errh_Error( "ERROR init Pb Master %s", ap->Name);
ctx->Node->EmergBreakTrue = 1;
return IO__ERRDEVICE;
/* Allocate area for local data structure */
ap->Local = calloc(1, sizeof(io_sAgentLocal));
if (!ap->Local) {
errh_Info( "ERROR config Profibus DP Master %s - %s", ap->Name, "calloc");
return IO__ERRINIDEVICE;
}
/* Initialize CMI */
if (op->Status < 1) {
sts = pb_cmi_init(local_master->Pb_fp);
if (sts != PB_OK) return IO__ERRINIDEVICE;
local = (io_sAgentLocal *) ap->Local;
if (op->DisableBus != 1) {
/* Open Pb driver */
sprintf(devname, "/dev/pbus%1d", op->BusNumber);
local->Pb_fp = open(devname, O_RDWR);
if (local->Pb_fp == -1)
{
/* Can't open driver */
errh_Info( "ERROR config Profibus DP Master %s - %s", ap->Name, "open device");
ctx->Node->EmergBreakTrue = 1;
return IO__ERRDEVICE;
}
/* Initialize CMI */
sts = pb_cmi_init(local->Pb_fp);
if (sts != PB_OK) {
errh_Info( "ERROR config Profibus DP Master %s - %s", ap->Name, "cmi init");
return IO__ERRINIDEVICE;
}
/* Set FMB configuration */
sts = fmb_set_configuration(local_master->Pb_fp, op);
if (sts != PB_OK) return IO__ERRINIDEVICE;
sts = fmb_set_configuration(local->Pb_fp, op);
if (sts != PB_OK) {
errh_Info( "ERROR config Profibus DP Master %s - %s", ap->Name, "fmb set configuration");
return IO__ERRINIDEVICE;
}
/* Set DP master parameters */
sts = dp_init_master(local_master->Pb_fp, op);
if (sts != PB_OK) return IO__ERRINIDEVICE;
sts = dp_init_master(local->Pb_fp, op);
if (sts != PB_OK) {
errh_Info( "ERROR config Profibus DP Master %s - %s", ap->Name, "dp init master");
return IO__ERRINIDEVICE;
}
/* Set DP bus parameters */
sts = dp_init_bus(local_master->Pb_fp, op);
if (sts != PB_OK) return IO__ERRINIDEVICE;
sts = dp_init_bus(local->Pb_fp, op);
if (sts != PB_OK) {
errh_Info( "ERROR config Profibus DP Master %s - %s", ap->Name, "dp init bus");
return IO__ERRINIDEVICE;
}
/* Move to STOP mode */
sts = act_param_loc(local_master->Pb_fp, op, DP_OP_MODE_STOP);
if (sts != PB_OK) errh_Info( "ERROR Init Pb Master %s", ap->Name );
}
op->Status = 1;
sts = act_param_loc(local->Pb_fp, op, DP_OP_MODE_STOP);
if (sts != PB_OK) {
errh_Info( "ERROR config Profibus DP Master %s - %s", ap->Name, "act param loc to STOPPED");
return IO__ERRINIDEVICE;
}
slavep = ap->racklist;
op->Status = PB_MASTER_STATE_STOPPED;
op->NumberSlaves = 0;
while (slavep) {
/* Allocate area for local data structure */
local_slave = calloc(1, sizeof(*local_slave));
slavep->Local = local_slave;
local_slave->initialized = 0;
op->NumberSlaves++;
slavep = slavep->next;
}
/* Loop through all slaves (traverse agent's children) and initialize them */
errh_Info( "Init Pb Master %s", ap->Name );
op->NumberSlaves = 0;
status = gdh_GetChild(ap->Objid, &slave_objid);
while (ODD(status)) {
status = gdh_GetObjectClass(slave_objid, &slave_class);
if (slave_class == pwr_cClass_Pb_DP_Slave) {
status = init_dp_slave(ap, slave_objid);
op->NumberSlaves++;
}
status = gdh_GetNextSibling(slave_objid, &slave_objid);
}
/* Move to CLEAR and OPERATE mode */
sts = act_param_loc(local->Pb_fp, op, DP_OP_MODE_CLEAR);
if (sts == PB_OK) {
op->Status = PB_MASTER_STATE_CLEARED;
sts = act_param_loc(local->Pb_fp, op, DP_OP_MODE_OPERATE);
if (sts == PB_OK) {
op->Status = PB_MASTER_STATE_OPERATE;
errh_Info( "Profibus DP Master %s to state OPERATE", ap->Name);
}
else {
errh_Info( "ERROR config Profibus DP Master %s - %s", ap->Name, "act param loc to OPERATE");
return IO__ERRINIDEVICE;
}
}
else {
errh_Info( "ERROR config Profibus DP Master %s - %s", ap->Name, "act param loc to CLEAR");
return IO__ERRINIDEVICE;
}
}
return IO__SUCCESS;
}
/*----------------------------------------------------------------------------*\
Read method for the Pb_Profiboard agent
......@@ -320,26 +454,38 @@ static pwr_tStatus IoAgentRead (
local = (io_sAgentLocal *) ap->Local;
op = (pwr_sClass_Pb_Profiboard *) ap->op;
// The write method for Pb agent is used to move the board to the OPERATE state. The
// reason why this isn't done in the init routine is that at least one slave has
// to be configurated before the board can move from the STOP state.
if (op->Status < 2 || op->Status > 3) {
/* Move to CLEAR mode */
sts = act_param_loc(local->Pb_fp, op, DP_OP_MODE_CLEAR);
if (sts == PB_OK) op->Status = 2;
errh_Info( "Pb Master, to CLEAR, %d", sts );
/* If everything is fine we should be in state OPERATE
Make a poll to see if there are diagnostics, the answer also tell us
if there are any hardware faults. In that case, make a reset and a new init. */
/* Move to OPERATE mode, this actually starts the bus communication */
sts = act_param_loc(local->Pb_fp, op, DP_OP_MODE_OPERATE);
if (sts == PB_OK) op->Status = 3;
errh_Info( "Pb Master, to OPERATE, %d", sts );
}
if (op->DisableBus != 1) {
switch (op->Status) {
case PB_MASTER_STATE_OPERATE:
sts = pb_cmi_poll(local->Pb_fp, NULL, NULL, NULL);
sts = pb_cmi_poll(local->Pb_fp, NULL, NULL, NULL);
/* In case of device error, move to state NOTINIT */
if (sts == PB_DEVICE_ERROR) {
op->Status = PB_MASTER_STATE_NOTINIT;
}
/* In case of diagnostic message, just mark it.
in the future, take care of it */
else if (sts != PB_NO_CON_IND_RECEIVED) {
op->Diag[0]++;
}
break;
default:
op->Status = PB_MASTER_STATE_NOTINIT;
errh_Info( "Reconfig of Profibus DP Master %s", ap->Name );
IoAgentClose(ctx, ap);
IoAgentInit(ctx, ap);
break;
}
if (sts != PB_NO_CON_IND_RECEIVED) op->Diag[0]++;
}
return IO__SUCCESS;
}
......@@ -352,12 +498,6 @@ static pwr_tStatus IoAgentWrite (
io_sAgent *ap
)
{
io_sAgentLocal *local;
pwr_sClass_Pb_Profiboard *op;
local = (io_sAgentLocal *) ap->Local;
op = (pwr_sClass_Pb_Profiboard *) ap->op;
return IO__SUCCESS;
}
......@@ -381,7 +521,6 @@ static pwr_tStatus IoAgentClose (
return IO__SUCCESS;
}
/*----------------------------------------------------------------------------*\
Every method to be exported to the workbench should be registred here.
......
src/lib/rt/src/os_linux/rt_io_pb_locals.h
View file @ 194563c4
#define IO_MAXCHAN 32
#define PB_SLAVE_STATE_NOTINIT 0
#define PB_SLAVE_STATE_STOPPED 1
#define PB_SLAVE_STATE_OPERATE 2
#define PB_MASTER_STATE_NOTINIT 0
#define PB_MASTER_STATE_STOPPED 1
#define PB_MASTER_STATE_CLEARED 2
#define PB_MASTER_STATE_OPERATE 3
#define PB_STALLACTION_NONE 0
#define PB_STALLACTION_RESET 1
#define PB_STALLACTION_BREAK 2
#define PB_NUMREP_UNSIGNEDINT 0
#define PB_NUMREP_SIGNEDINT 1
#define PB_NUMREP_FLOATIEEE 2
#define PB_NUMREP_FLOATVAX 3
#define PB_NUMREP_FLOATINTEL 4
#define PB_BYTEORDERING_LE 0
#define PB_BYTEORDERING_BE 1
#define PB_ORIENTATION_BYTE 8
#define PB_ORIENTATION_WORD 16
#define PB_ORIENTATION_DWORD 32
typedef struct {
int Pb_fp;
} io_sAgentLocal;
......@@ -9,10 +35,8 @@ typedef struct {
} io_sRackLocal;
typedef struct {
int initialized;
void *input_area;
void *output_area;
short byte_swap;
int initialized;
int scancount[IO_MAXCHAN];
short unsign_rawval;
} io_sCardLocal;
src/lib/rt/src/os_linux/rt_io_profiboard.c
View file @ 194563c4
......@@ -146,6 +146,8 @@ pwr_tUInt16 pb_cmi_poll(int fp,
crs.retval_ptr = &retval;
sts = ioctl(fp, PB_IOCTL_CMI_READ, (char *) &crs);
if (sts != 0) return PB_DEVICE_ERROR;
if (retval == CON_IND_RECEIVED) {
......@@ -222,6 +224,7 @@ pwr_tUInt16 pb_cmi_get_data(int fp,
retry_counter = 10;
do {
read(fp, (char *) &user_arg, data_size);
if (retval == E_IF_FATAL_ERROR) break;
}
while(retval != E_OK && retry_counter-- > 0);
......@@ -250,6 +253,7 @@ pwr_tUInt16 pb_cmi_set_data(int fp,
retry_counter = 10;
do {
write(fp, (char *) &user_arg, data_size);
if (retval == E_IF_FATAL_ERROR) break;
}
while(retval != E_OK && retry_counter-- > 0);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment
GitLab Nexedi Edition | About GitLab | About Nexedi | 沪ICP备2021021310号-2 | 沪ICP备2021021310号-7