You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
454 lines
13 KiB
454 lines
13 KiB
|
|
|
|
#include "modelbahn.h"
|
|
#include "interface.h"
|
|
#include "interface-z21.h"
|
|
|
|
//
|
|
// command and datasets for communication
|
|
static unsigned char TX_X_SET_TRACK_POWER_ON[] { 0x07, 0x00, 0x40, 0x00, 0x21, 0x81, 0xa0 };
|
|
static unsigned char TX_X_SET_TRACK_POWER_OFF[] { 0x07, 0x00, 0x40, 0x00, 0x21, 0x80, 0xa1 };
|
|
|
|
//
|
|
// values to comapare for data
|
|
static unsigned char RX_SYSTEMSTATE_DATACHANGED[] { 0x14, 0x00, 0x84, 0x00 };
|
|
static unsigned char RX_GET_SERIAL_NUMBER[] { 0x08, 0x00, 0x10, 0x00 };
|
|
static unsigned char RX_LOCONET_Z21_TX[] { 0x00, 0xa1, 0x00 }; // loconet dynamic length
|
|
static unsigned char RX_X_GET_TURNOUT_INFO[] { 0x09, 0x00, 0x40, 0x00, 0x43 };
|
|
static unsigned char RX_RMBUS_DATACHANGED[] { 0x0F, 0x00, 0x80, 0x00 };
|
|
|
|
//
|
|
// locomotive values
|
|
#define Z21_IDX_SETLOCO_DRIVE_SFMT 5
|
|
#define Z21_IDX_SETLOCO_DRIVE_ADRH 6
|
|
#define Z21_IDX_SETLOCO_DRIVE_ADRL 7
|
|
#define Z21_IDX_SETLOCO_DRIVE_STEP 8
|
|
#define Z21_IDX_SETLOCO_DRIVE_CHK 9
|
|
|
|
//
|
|
// turnout values
|
|
#define Z21_IDX_SETTURNOUT_ADRH 5
|
|
#define Z21_IDX_SETTURNOUT_ADRL 6
|
|
#define Z21_IDX_SETTURNOUT_MODE 7
|
|
#define Z21_IDX_SETTURNOUT_CHK 8
|
|
|
|
InterfaceZ21::InterfaceZ21 () {
|
|
status_connected = false;
|
|
status_poweron = false;
|
|
status_programmingmode = false;
|
|
status_shortcircuit = false;
|
|
status_emergencystop = false;
|
|
|
|
send_logon = false;
|
|
|
|
memset (rmsensors, 0x0, INTF_Z21_RMSENSOR_GROUPS * INTF_Z21_RMSENSOR_BYTES);
|
|
rmsensorinit = 0;
|
|
|
|
serial = "";
|
|
hostname = "";
|
|
timeout = time(NULL);
|
|
rmgetdatatimeout = time(NULL) - INTF_Z21_RMGETDATA_TIMEOUT;
|
|
};
|
|
|
|
InterfaceZ21::~InterfaceZ21() {
|
|
Disconnect();
|
|
};
|
|
|
|
|
|
void InterfaceZ21::Connect (string destination) {
|
|
debug (DEBUG_INFO | DEBUG_IFACE, "%s:%d Connect to: %s", __FILE__, __LINE__, destination.c_str());
|
|
|
|
if (status_connected) Disconnect();
|
|
hostname = destination;
|
|
|
|
if (udp.Listen(0) == 0) {
|
|
debug (DEBUG_ERROR | DEBUG_IFACE, "%s:%d Error could not bind UDP socket (%s)",
|
|
__FILE__, __LINE__, strerror(errno));
|
|
return;
|
|
}
|
|
udp.SetBlocked(1);
|
|
timeout = time(NULL);
|
|
send_SET_BROADCASTFLAGS();
|
|
send_GET_SERIAL_NUMBER();
|
|
status_connected = true;
|
|
|
|
for (int i = 0; i < INTF_Z21_LOCONET_MAXADDR; i++) {
|
|
loconet_map[i] = { 0 , 0, 0, 0, 0};
|
|
}
|
|
};
|
|
|
|
|
|
|
|
//
|
|
// if connected, poweroff track and logoff from Z21 controller
|
|
void InterfaceZ21::Disconnect() {
|
|
if (status_connected) {
|
|
PowerOnOff(0);
|
|
send_LOGOFF();
|
|
}
|
|
udp.Close();
|
|
|
|
status_connected = false;
|
|
status_poweron = false;
|
|
};
|
|
|
|
///////////////////////////////////////////////////////////////////////
|
|
//
|
|
//
|
|
// check if we are connected and for timeouts.
|
|
// read and go through incoming data
|
|
int InterfaceZ21::Loop(string interfacename) {
|
|
time_t curtime = time(NULL);
|
|
string source;
|
|
int inlen;
|
|
int i;
|
|
int update = false;
|
|
|
|
if (status_connected) {
|
|
//
|
|
// Z21 timed out? close all connection
|
|
if ((timeout + 2 * INTF_Z21_TIMEOUT) < curtime) {
|
|
debug (DEBUG_ERROR | DEBUG_IFACE, "%s:%d connection timed out (%d)", __FILE__, __LINE__, curtime-(timeout + 2 * INTF_Z21_TIMEOUT));
|
|
Disconnect();
|
|
update = true;
|
|
}
|
|
else if (send_logon == false && (timeout + INTF_Z21_TIMEOUT) < curtime) {
|
|
send_logon = true;
|
|
send_GET_SERIAL_NUMBER();
|
|
send_SET_BROADCASTFLAGS();
|
|
timeout = time(NULL);
|
|
update = true;
|
|
}
|
|
|
|
//
|
|
// rmgetdatatimeout?
|
|
if (curtime - rmgetdatatimeout > INTF_Z21_RMGETDATA_TIMEOUT) {
|
|
rmgetdatatimeout = curtime;
|
|
send_RM_GETDATA(0);
|
|
send_RM_GETDATA(1);
|
|
}
|
|
}
|
|
|
|
|
|
//
|
|
// check for incoming data
|
|
if (status_connected) {
|
|
if ((inlen = udp.ReadTimeout(&source, inbuffer, INTF_Z21_INBUFFER, 10)) > 0) {
|
|
//
|
|
// got some data
|
|
|
|
//
|
|
// check for LAN_SYSTEMSTATE_DATACHANGED
|
|
if (memcmp (inbuffer, RX_GET_SERIAL_NUMBER, sizeof(RX_GET_SERIAL_NUMBER)) == 0) {
|
|
// got serial number... ignore this for now
|
|
send_logon = false;
|
|
}
|
|
|
|
//
|
|
// check for LAN_X_TURNOUT_INFO
|
|
if (memcmp (inbuffer, RX_X_GET_TURNOUT_INFO, sizeof(RX_X_GET_TURNOUT_INFO)) == 0) {
|
|
// got turnout information
|
|
int addr = 0;
|
|
|
|
addr = ((unsigned char)inbuffer[Z21_IDX_SETTURNOUT_ADRH] << 8) + (unsigned char)inbuffer[Z21_IDX_SETTURNOUT_ADRL];
|
|
|
|
if (inbuffer[Z21_IDX_SETTURNOUT_MODE] == 2)
|
|
server->TurnoutAddrMode(interfacename, addr, 1);
|
|
else if (inbuffer[Z21_IDX_SETTURNOUT_MODE] == 1)
|
|
server->TurnoutAddrMode(interfacename, addr, 0);
|
|
}
|
|
|
|
|
|
|
|
//
|
|
// check for LAN_SYSTEMSTATE_DATACHANGED
|
|
else if (memcmp (inbuffer, RX_SYSTEMSTATE_DATACHANGED, sizeof(RX_SYSTEMSTATE_DATACHANGED)) == 0) {
|
|
int cs = (unsigned char) inbuffer[16];
|
|
int csex = (unsigned char) inbuffer[17];
|
|
|
|
bool old_poweron = status_poweron;
|
|
bool old_shortcircuit = status_shortcircuit;
|
|
bool old_programmingmode = status_programmingmode;
|
|
bool old_connected = status_connected;
|
|
bool old_emergencystop = status_emergencystop;
|
|
|
|
status_emergencystop = (cs & INTF_Z21_CS_EmergencyStop);
|
|
status_poweron = (cs & INTF_Z21_CS_TrackVoltageOff);
|
|
status_shortcircuit = (cs & INTF_Z21_CS_ShortCircuit);
|
|
status_programmingmode = (cs & INTF_Z21_CS_ProgModeActive);
|
|
|
|
debug (0, "%s:%d cs:%d csex:%d", __FILE__, __LINE__, cs, csex);
|
|
|
|
update = true;
|
|
//if ( old_poweron != status_poweron ||
|
|
// old_shortcircuit != status_shortcircuit ||
|
|
// old_programmingmode != status_programmingmode ||
|
|
// old_emergencystop != status_emergencystop) update = true;
|
|
}
|
|
|
|
//
|
|
// LOCONET
|
|
//Got some Data:08:00:a1:00:a1:02:00:5c:
|
|
//Got some Data:08:00:a1:00:a0:02:55:08:
|
|
else if (memcmp (inbuffer+1, RX_LOCONET_Z21_TX, sizeof (RX_LOCONET_Z21_TX)) == 0) {
|
|
int loconetopc = (unsigned char)inbuffer[4];
|
|
printf ("%s:%d Got some LOCONET (%x) Data:", __FILE__, __LINE__, loconetopc);
|
|
for (i = 0; i < inlen; i++) {
|
|
int z = (unsigned char) inbuffer[i];
|
|
printf ("i:%d %02x:", i, z);
|
|
}
|
|
printf ("\n");
|
|
|
|
//
|
|
//
|
|
if (loconetopc == 0xa0) {
|
|
//
|
|
// speed
|
|
unsigned int inloc = inbuffer[5];
|
|
unsigned int inspeed = inbuffer[6];
|
|
|
|
if (inloc >= 0 && inloc <= INTF_Z21_LOCONET_MAXADDR) {
|
|
if (loconet_last.addr) {
|
|
debug (0, "%s:%d loconet_lastaddr:%d", __FILE__, __LINE__, loconet_last.addr);
|
|
loconet_map[inloc].addr = loconet_last.addr;
|
|
}
|
|
|
|
debug (0, "%s:%d Loc:%d Speed:%d", __FILE__, __LINE__, loconet_map[inloc].addr, inspeed);
|
|
if (loconet_map[inloc].addr > 0) {
|
|
int speed = inspeed;
|
|
server->LocomotiveAddrSpeed(interfacename, loconet_map[inloc].addr, speed);
|
|
}
|
|
}
|
|
loconet_last.addr = 0;
|
|
}
|
|
|
|
if (loconetopc == 0xa1) {
|
|
//
|
|
// speed
|
|
unsigned int inloc = inbuffer[5];
|
|
unsigned int infunc = inbuffer[6];
|
|
|
|
if (inloc >= 0 && inloc <= INTF_Z21_LOCONET_MAXADDR) {
|
|
if (loconet_last.addr) loconet_map[inloc] = loconet_last;
|
|
debug (0, "%s:%d Loc:%d Function:%d", __FILE__, __LINE__, loconet_map[inloc].addr, infunc);
|
|
}
|
|
|
|
debug (0, "%s:%d Loc:%d Function:%d", __FILE__, __LINE__, loconet_map[inloc].addr, infunc);
|
|
if (loconet_map[inloc].addr > 0) {
|
|
server->LocomotiveAddrFunction(interfacename, loconet_map[inloc].addr, infunc);
|
|
}
|
|
|
|
loconet_last.addr = 0;
|
|
}
|
|
}
|
|
|
|
//
|
|
// check for LAM_RMBUS_DATACHANGED
|
|
else if (memcmp (inbuffer, RX_RMBUS_DATACHANGED, sizeof(RX_RMBUS_DATACHANGED)) == 0) {
|
|
int group = (unsigned char)inbuffer[4];
|
|
int idx, bit;
|
|
|
|
printf ("LAN_RMBUS_DATA_CHANGE[%s] ", interfacename.c_str());
|
|
for (idx = 0; idx < INTF_Z21_RMSENSOR_BYTES * INTF_Z21_RMSENSOR_GROUPS; idx++) {
|
|
printf ("%x ", (unsigned char) inbuffer[4+idx]);
|
|
}
|
|
printf ("\n");
|
|
|
|
// groupindex (first or last 10 bytes)
|
|
unsigned char rmsold[INTF_Z21_RMSENSOR_BYTES * INTF_Z21_RMSENSOR_GROUPS];
|
|
memcpy (rmsold, rmsensors, INTF_Z21_RMSENSOR_BYTES * INTF_Z21_RMSENSOR_GROUPS);
|
|
|
|
if (group < INTF_Z21_RMSENSOR_GROUPS) {
|
|
memcpy (rmsensors+(group * INTF_Z21_RMSENSOR_BYTES), inbuffer+5, INTF_Z21_RMSENSOR_BYTES);
|
|
|
|
// check for changes
|
|
for (idx = 0; idx < INTF_Z21_RMSENSOR_GROUPS*INTF_Z21_RMSENSOR_BYTES; idx++) {
|
|
if (rmsold[idx]^rmsensors[idx]) {
|
|
for (i = 0; i < 8; i++) if (rmsensorinit || (rmsold[idx] & 1 << i) != (rmsensors[idx] & 1 << i)) {
|
|
debug (0, "Sendor Data Changed: %s[%d]", interfacename.c_str(), idx*8+i);
|
|
if (rmsensors[idx] & 1 << i) server->SensorAddrChange(interfacename, idx*8+i, 1);
|
|
else server->SensorAddrChange(interfacename, idx*8+i, 0);
|
|
}
|
|
}
|
|
}
|
|
rmsensorinit = 0;
|
|
}
|
|
}
|
|
|
|
else {
|
|
printf ("InterfaceZ21(%s) Got some Data:", interfacename.c_str());
|
|
for (i = 0; i < inlen; i++) {
|
|
int z = (unsigned char) inbuffer[i];
|
|
printf ("%02x:", z);
|
|
}
|
|
printf ("\n");
|
|
}
|
|
}
|
|
else if (inlen < 0) {
|
|
if (errno != EAGAIN && errno != EWOULDBLOCK) {
|
|
debug (DEBUG_ERROR | DEBUG_IFACE, "%s:%d error on reading (%s)",
|
|
__FILE__, __LINE__, strerror(errno));
|
|
}
|
|
}
|
|
}
|
|
|
|
return update;
|
|
};
|
|
|
|
|
|
//
|
|
// send_SET_BROADCASTFLAGS();
|
|
void InterfaceZ21::send_SET_BROADCASTFLAGS() {
|
|
unsigned char buffer[] = { 0x08, 0x00, 0x50, 0x00, 0x0F, 0x01, 0x00, 0x03 };
|
|
udp.Write(hostname, (char*)buffer, sizeof (buffer));
|
|
}
|
|
|
|
//
|
|
// send_GET_SERIAL_NUMBER
|
|
void InterfaceZ21::send_GET_SERIAL_NUMBER() {
|
|
unsigned char buffer[] = { 0x04, 0x00, 0x10, 0x00 };
|
|
udp.Write(hostname, (char*)buffer, sizeof (buffer));
|
|
}
|
|
|
|
|
|
//
|
|
// send_RM_GETDATA
|
|
void InterfaceZ21::send_RM_GETDATA(int group) {
|
|
unsigned char buffer[] = { 0x05, 0x00, 0x81, 0x00, 0x00 };
|
|
if (group < INTF_Z21_RMSENSOR_GROUPS) {
|
|
buffer[4] = (unsigned char)group;
|
|
udp.Write(hostname, (char*)buffer, sizeof (buffer));
|
|
}
|
|
}
|
|
|
|
|
|
//
|
|
// send_LOGOFF
|
|
void InterfaceZ21::send_LOGOFF() {
|
|
if (status_connected == false) return;
|
|
|
|
unsigned char buffer[] = { 0x04, 0x00, 0x30, 0x00 };
|
|
udp.Write(hostname, (char*)buffer, sizeof (buffer));
|
|
}
|
|
|
|
|
|
//
|
|
// Set Locomotive Speed
|
|
void InterfaceZ21::SetLocoSpeed(Locomotive *l, int step) {
|
|
unsigned char buffer[] = { 0x0A, 0x00, 0x40, 0x00, 0xE4, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00 };
|
|
|
|
debug (0, "%s:%d InterfaceZ21::SetLocoSpeed step:%d", __FILE__, __LINE__, step);
|
|
|
|
//
|
|
// reverse?
|
|
if (step < 0) {
|
|
l->flags |= LOCO_F_REVERSE;
|
|
}
|
|
else if (step > 0) {
|
|
l->flags &= ~LOCO_F_REVERSE;
|
|
}
|
|
if (l->flags & LOCO_F_REVERSE) buffer[Z21_IDX_SETLOCO_DRIVE_STEP] = 0x00;
|
|
else buffer[Z21_IDX_SETLOCO_DRIVE_STEP] = 0x80;
|
|
|
|
//
|
|
// setup loc addr
|
|
if (l->addr >= 128) buffer[Z21_IDX_SETLOCO_DRIVE_ADRH] = 0xC0;
|
|
else buffer[Z21_IDX_SETLOCO_DRIVE_ADRH] = 0;
|
|
buffer[Z21_IDX_SETLOCO_DRIVE_ADRL] = (unsigned char) (l->addr & 0xFF);
|
|
buffer[Z21_IDX_SETLOCO_DRIVE_ADRH] |= (unsigned char) (l->addr >> 8);
|
|
|
|
//
|
|
// setup steps and selected step
|
|
if (l->stepcode == LOCO_INT_DCC14) {
|
|
buffer[Z21_IDX_SETLOCO_DRIVE_SFMT] = 0x10;
|
|
if (step == 0) buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= 0;
|
|
else buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= (abs(step)+1);
|
|
}
|
|
else if (l->stepcode == LOCO_INT_DCC28) {
|
|
buffer[Z21_IDX_SETLOCO_DRIVE_SFMT] = 0x12;
|
|
if (step == 0) buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= 0;
|
|
else {
|
|
buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= ((abs(step)>>1)+1);
|
|
if (abs(step) & 0x01) buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= 0x10;
|
|
}
|
|
}
|
|
else if (l->stepcode == LOCO_INT_DCC128) {
|
|
buffer[Z21_IDX_SETLOCO_DRIVE_SFMT] = 0x13;
|
|
if (step == 0) buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= 0;
|
|
else buffer[Z21_IDX_SETLOCO_DRIVE_STEP] |= (abs(step)+1);
|
|
}
|
|
|
|
//
|
|
// XOR Byte
|
|
for (int i = 4; i < sizeof (buffer)-1; i++)
|
|
buffer[Z21_IDX_SETLOCO_DRIVE_CHK] = (unsigned char)buffer[Z21_IDX_SETLOCO_DRIVE_CHK] xor (unsigned char)buffer[i];
|
|
|
|
printf ("Send Data:");
|
|
for (int i = 0; i < sizeof (buffer); i++) {
|
|
int z = (unsigned char) buffer[i];
|
|
printf ("%02x:", z);
|
|
}
|
|
printf ("\n");
|
|
udp.Write(hostname, (char*)buffer, sizeof (buffer));
|
|
loconet_last.addr = l->addr;
|
|
loconet_last.dir = l->flags & LOCO_F_REVERSE;
|
|
loconet_last.speed = l->speed;
|
|
loconet_last.maxspeed = l->vmax;
|
|
loconet_last.stepcode = l->stepcode;
|
|
};
|
|
|
|
|
|
//
|
|
// Set Locomotive Function
|
|
void InterfaceZ21::SetLocoFunction(Locomotive *l, int func, int value) {
|
|
debug (0, "%s:%d InterfaceZ21::SetLocoFunction", __FILE__, __LINE__);
|
|
};
|
|
|
|
|
|
//
|
|
// Set Turnout
|
|
void InterfaceZ21::SetTurnout(Turnout *t, int activate, int motoractive) {
|
|
unsigned char buffer[] = { 0x09, 0x00, 0x40, 0x00, 0x53, 0x00, 0x00, 0x00, 0x00 };
|
|
|
|
debug (0, "%s:%d InterfaceZ21::SetTurnout (a:%d, m:%d)", __FILE__, __LINE__, activate, motoractive);
|
|
|
|
//
|
|
// setup turnout addr
|
|
buffer[Z21_IDX_SETTURNOUT_ADRL] = (unsigned char) (t->addr & 0xFF);
|
|
buffer[Z21_IDX_SETTURNOUT_ADRH] |= (unsigned char) (t->addr >> 8);
|
|
|
|
//
|
|
// setup steps and selected step
|
|
buffer[Z21_IDX_SETTURNOUT_MODE] = 0x80;
|
|
if (activate) buffer[Z21_IDX_SETTURNOUT_MODE] |= 1;
|
|
if (motoractive) buffer[Z21_IDX_SETTURNOUT_MODE] |= 8;
|
|
|
|
//
|
|
// XOR Byte
|
|
for (int i = 4; i < sizeof (buffer)-1; i++)
|
|
buffer[Z21_IDX_SETTURNOUT_CHK] = (unsigned char)buffer[Z21_IDX_SETTURNOUT_CHK] xor (unsigned char)buffer[i];
|
|
|
|
printf ("Send Data:");
|
|
for (int i = 0; i < sizeof (buffer); i++) {
|
|
int z = (unsigned char) buffer[i];
|
|
printf ("%02x:", z);
|
|
}
|
|
printf ("\n");
|
|
udp.Write(hostname, (char*)buffer, sizeof (buffer));
|
|
};
|
|
|
|
|
|
//
|
|
// power on off
|
|
// send_X_SET_TRACK_POWER_ON(); send_X_SET_TRACK_POWER_ON();
|
|
void InterfaceZ21::PowerOnOff(int onoff) {
|
|
if (status_connected == false) return;
|
|
|
|
if (onoff) {
|
|
udp.Write(hostname, (char*)TX_X_SET_TRACK_POWER_ON, sizeof (TX_X_SET_TRACK_POWER_ON));
|
|
}
|
|
else {
|
|
udp.Write(hostname, (char*)TX_X_SET_TRACK_POWER_OFF, sizeof (TX_X_SET_TRACK_POWER_OFF));
|
|
}
|
|
};
|