BlockEnterNext is not working correctly

master
Steffen Pohle 2 years ago
parent 48922c20ea
commit 4a4735ab33

@ -251,7 +251,7 @@ int Blocks::SetLockedby (string blname, string lockedby, int lock_onoff) {
Block *bl = NULL;
JSONParse jp;
int res = -1;
int x, y;
int x, y, cnt;
debug (0, "Blocks::SetLockedby block:'%s' locked for '%s' locked:%d", blname.c_str(), lockedby.c_str(), lock_onoff);
@ -264,7 +264,8 @@ int Blocks::SetLockedby (string blname, string lockedby, int lock_onoff) {
if(network) network->ChangeListPushToAll(jp.ToString());
x = -1; y = -1;
while (server->railways.FindReference(&x, &y, blname)) {
cnt = 0;
while (server->railways.FindReference(&x, &y, blname, (cnt++))) {
server->railways.SetLockedby(x, y, lockedby, lock_onoff);
}

@ -362,7 +362,8 @@ int Locomotives::SetAssign (string name, string block, int direction) {
UnLock();
if (i < max && i >=0) {
while (server->railways.FindReference(&x, &y, block)) {
int cnt = 0;
while (server->railways.FindReference(&x, &y, block, (cnt++))) {
string bl = server->blocks.GetLockedby(block);
if(bl.length() == 0 || bl.compare(name) == 0) {
server->blocks.SetLockedby(block, name, 1);
@ -700,20 +701,25 @@ int Locomotives::SchedulerNextStep(Locomotive *loc) {
//
// with each call only check one single step of the way, if we have to
// turn a turnout try to do it and return 0
// if everything is set up till the destination block return 1
// turn a turnout try to do it and return 0.
// if everything is set up till the destination (next) block return 1
// to speed things up: we only prepare ways wich we have locked already
//
int Locomotives::AutoCheckWaySingleStep(string way, string locname, int *data) {
int res = 0;
size_t pos1;
size_t curpos;
size_t blockpos;
int cnt;
int state;
int newdata = 0;
string turnout;
int x, y;
Railway r;
if (*data < 0) *data = 0;
debug (0, "Locomotives::AutoCheckWaySingleStep Way:%s", way.c_str());
debug (0, "Locomotives::AutoCheckWaySingleStep Prepare for Loco: %s Way:%s", locname.c_str(), way.c_str());
curpos = 0;
do {
@ -723,28 +729,45 @@ int Locomotives::AutoCheckWaySingleStep(string way, string locname, int *data) {
pos1 = way.find(":", curpos+3);
if (pos1 == string::npos) {
debug (0, "%s:%d turnout without value? '%s'", __FILE__, __LINE__, way.c_str());
server->PowerOnOff(0);
server->SetModeManual();
return 0;
}
turnout = way.substr(curpos+3, pos1-curpos-3);
state = server->turnouts.Get(way.substr(curpos+3, pos1-curpos-3));
state = server->turnouts.Get(turnout);
if (state == -1) {
debug (0, "%s:%d turnout not found '%s'", __FILE__, __LINE__, way.substr(curpos+3, pos1-curpos-3).c_str());
debug (0, "%s:%d turnout not found '%s'", __FILE__, __LINE__, turnout.c_str());
server->PowerOnOff(0);
server->SetModeManual();
return 0;
}
if (way[pos1+1] == '0' && state != 0) {
server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 0);
break;
}
else if (way[pos1+1] == '1' && state != 1) {
server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 1);
break;
cnt = 0;
while (server->railways.FindReference(&x, &y, turnout, cnt++)) {
cnt++;
r = server->railways.RailwayGet(x, y);
debug (0, "Locomotives::AutoCheckWaySingleStep Loco:%s Turnout: %s LockedBy:%s", locname.c_str(), turnout.c_str(), r.lockedby);
if (locname.compare(r.lockedby) == 0) {
if (way[pos1+1] == '0' && state != 0) {
server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 0);
break;
}
else if (way[pos1+1] == '1' && state != 1) {
server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 1);
break;
}
else if (newdata > *data) {
if (way[pos1+1] == '0') server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 0);
if (way[pos1+1] == '1') server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 1);
(*data) = newdata;
break;
}
}
}
else if (newdata > *data) {
if (way[pos1+1] == '0') server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 0);
if (way[pos1+1] == '1') server->turnouts.Set(way.substr(curpos+3, pos1-curpos-3), 1);
(*data) = newdata;
break;
if (cnt == 0) {
debug (0, "Locomotives::AutoCheckWaySingleStep Loco:%s Turnout:%s Reference not found", locname.c_str(), turnout.c_str());
}
}
} while (curpos != string::npos);
@ -932,7 +955,8 @@ int Locomotives::Loco_OnRoute(Locomotive *loco) {
if (Loco_SearchAndLock(loco) == 1) {
loco->auto_onroute = LOCO_OR_ENTERBLOCKNEXT;
debug (0, "Locomotives::Loco_OnRoute Found Way Prepare '%s'\n", loco->name);
SetSpeed(loco->name, loco->flags & LOCO_F_REVERSE ? -loco->vmin : loco->vmid);
SetSpeed(loco->name, loco->flags & LOCO_F_REVERSE ? -loco->vslow : loco->vslow);
timer_start(&loco->auto_timenext);
}
else {
loco->auto_onroute = LOCO_OR_ENTERBLOCKSTOP;
@ -1012,13 +1036,18 @@ int Locomotives::Loco_BlockEnterNext(Locomotive *loco) {
s_enter = server->blocks.GetSensorEnter(dir_reverse, block, loco->flags);
if (server->sensors.GetActive(s_stop) == 1) {
if (loco->auto_timenext.tv_sec > 0) {
if (!AutoCheckWaySingleStep(loco->auto_way, loco->name, &loco->auto_data)) {
//
// end of block and not finished preparing the way.
// stop everything
debug (0, "Locomotives::Loco_BlockEnterNext ********** Loco:%s could not prepare way in time. STOP", loco->name);
server->PowerOnOff(0);
debug (0, "Locomotives::Loco_BlockEnterNext could not prepare way in time '%s'\n", loco->name);
SetSpeed(loco->name, 0);
loco->auto_onroute = LOCO_OR_STOPWAIT;
server->railways.UnLockWay(loco->auto_wayold, loco->name);
server->blocks.SetLockedby(loco->blockprev+2, loco->name, 0);
loco->auto_wayold[0] = 0;
timer_start(&loco->auto_timenext);
return 0;
}
else {
@ -1035,14 +1064,14 @@ int Locomotives::Loco_BlockEnterNext(Locomotive *loco) {
}
if (loco->auto_timenext.tv_sec > 0 && timer_get(&loco->auto_timenext) > LOCO_TO_TURNOUT) {
debug (0, "* Locomotives::Loco_BlockEnterNext prepare way for loco: %s", loco->name);
if (AutoCheckWaySingleStep(loco->auto_way, loco->name, &loco->auto_data) == 1) {
//
// finished preparing new way, need to stay in ENTERBLOCKNEXT to unlock old way
debug (0, "* Locomotives::Loco_BlockEnterNext finished preparing way for loco: %s", loco->name);
loco->auto_timenext.tv_sec = 0;
}
else timer_start(&loco->auto_timenext);
debug (0, "* Locomotives::Loco_BlockEnterNext prepare way for loco: %s", loco->name);
if (AutoCheckWaySingleStep(loco->auto_way, loco->name, &loco->auto_data) == 1) {
//
// finished preparing new way, need to stay in ENTERBLOCKNEXT to unlock old way
debug (0, "* Locomotives::Loco_BlockEnterNext finished preparing way for loco: %s", loco->name);
loco->auto_timenext.tv_sec = 0;
}
else timer_start(&loco->auto_timenext);
}
return 0;

@ -11,6 +11,19 @@ Railways::Railways() {
mtx = { 0 };
mtx = PTHREAD_MUTEX_INITIALIZER;
changed = 0;
//
// do some tests on start
string way = "b:-:B43,t:w90:0,b:-:B1,t:w28:1,t:w30:1,t:w31:0,t:w32:0,t:w33:0,t:w33:0,t:w34:1,t:w35:0,t:w36:1,t:w31:1,t:w30:1,t:w28:1,b:+:B1,t:w90:1,b:+:B42a,t:w89:0,b:+:B42b,t:w91:1,b:+:BTest,t:w95:1,b:-:B31";
if (!FindWayCheckIfNoDoubleTurnouts(&way) == 0) {
printf ("%s:%d FindWayCheckIfNoDoubleTurnouts not ok\n", __FILE__, __LINE__);
raise (SIGINT);
}
way = "b:-:B43,t:w90:0,b:-:B1,t:w28:1,t:w32:0,t:w33:0,t:w33:0,t:w34:1,t:w35:0,t:w36:1,t:w31:1,t:w30:1,t:w28:1,b:+:B1,t:w90:1,b:+:B42a,t:w89:0,b:+:B42b,t:w91:1,b:+:BTest,t:w95:1,b:-:B31";
if (FindWayCheckIfNoDoubleTurnouts(&way) == 0) {
printf ("%s:%d FindWayCheckIfNoDoubleTurnouts not ok\n", __FILE__, __LINE__);
raise (SIGINT);
}
};
@ -42,6 +55,7 @@ int Railways::UnLock() {
struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype) {
struct s_findway_data np = { x: -1, y: -1, enterfrom: -1, oldx: pos.x, oldy:pos.y,
oldenterfrom: pos.enterfrom,
dist: pos.dist + 1,
way: pos.way }; // newpos
if (dirtype == 1) {
@ -49,10 +63,12 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype)
np.x = pos.x;
np.y = pos.y + 1 ;
np.enterfrom = EF_NORTH;
return np;
} else if (pos.enterfrom == EF_SOUTH) {
np.x = pos.x;
np.y = pos.y - 1;
np.enterfrom = EF_SOUTH;
return np;
}
}
else if (dirtype == 2) {
@ -60,10 +76,12 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype)
np.x = pos.x + 1;
np.y = pos.y;
np.enterfrom = EF_WEST;
return np;
} else if (pos.enterfrom == EF_EAST) {
np.x = pos.x - 1;
np.y = pos.y;
np.enterfrom = EF_EAST;
return np;
}
}
else if (dirtype == 3) {
@ -71,10 +89,12 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype)
np.x = pos.x;
np.y = pos.y - 1;
np.enterfrom = EF_SOUTH;
return np;
} else if (pos.enterfrom == EF_NORTH){
np.x = pos.x - 1;
np.y = pos.y;
np.enterfrom = EF_EAST;
return np;
}
}
else if (dirtype == 4) {
@ -82,10 +102,12 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype)
np.x = pos.x + 1;
np.y = pos.y;
np.enterfrom = EF_WEST;
return np;
} else if (pos.enterfrom == EF_EAST){
np.x = pos.x;
np.y = pos.y - 1;
np.enterfrom = EF_SOUTH;
return np;
}
}
else if (dirtype == 5) {
@ -93,10 +115,12 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype)
np.x = pos.x + 1;
np.y = pos.y;
np.enterfrom = EF_WEST;
return np;
} else if (pos.enterfrom == EF_EAST){
np.x = pos.x;
np.y = pos.y + 1;
np.enterfrom = EF_NORTH;
return np;
}
}
else if (dirtype == 6) {
@ -104,12 +128,18 @@ struct s_findway_data Railways::NextPos(struct s_findway_data pos, int dirtype)
np.x = pos.x;
np.y = pos.y + 1;
np.enterfrom = EF_NORTH;
return np;
} else if (pos.enterfrom == EF_SOUTH){
np.x = pos.x - 1;
np.y = pos.y;
np.enterfrom = EF_EAST;
return np;
}
}
else {
debug (0, "Railways::NextPos dirtype unknown:%d", dirtype);
return np;
}
return np;
};
@ -393,37 +423,32 @@ void Railways::ClearLockedby(string name) {
}
int Railways::_FindReference(int *x, int *y, string name) {
int Railways::_FindReference(int *x, int *y, string name, int cnt) {
int found = 0;
int c; // counter of occurance
if (x == NULL || y == NULL) return 0;
if (*x < 0 || *y < 0) {
*x = 0;
*y = 0;
}
for (; found == 0 && *y < height; (*y)++) {
if (*x >= width) *x = 0;
else (*x)++;
for (; found == 0 && *x < width; (*x)++) {
if (strncmp (railways[GetRIdx(*x, *y)].name, name.c_str(), REFERENCENAME_LEN) == 0) {
found = 1;
break;
}
for (c = 0, *y = 0; found == 0 && *y < height; (*y)++) {
for (*x = 0; found == 0 && *x < width; (*x)++) {
if (strncmp (railways[GetRIdx(*x, *y)].name, name.c_str(), REFERENCENAME_LEN) == 0)
if ((c++) == cnt) {
found = 1;
break;
}
}
if (found) break;
}
if (*x < width && *y < height) return 1;
else return 0;
return 0;
};
int Railways::FindReference(int *x, int *y, string name) {
int Railways::FindReference(int *x, int *y, string name, int cnt) {
int res;
Lock();
res = _FindReference (x, y, name);
res = _FindReference (x, y, name, cnt);
UnLock();
return res;
};
@ -519,6 +544,23 @@ string Railways::GetDestBlock(int locoflags, string blockend) {
return block;
}
/*
* insert the element in an ordered list
*/
void FDListPush (std::list<struct s_findway_data> *list, struct s_findway_data *element) {
std::list<struct s_findway_data>::iterator iter;
for (iter = list->begin(); iter != list->end(); iter++)
if (element->dist < iter->dist) {
// printf ("%s:%d Push dist list:%d dist element:%d (%d,%d E:%d)\n", __FILE__, __LINE__, iter->dist, element->dist, element->x, element->y, element->enterfrom);
list->insert(iter, *element);
return;
}
// printf ("%s:%d Push dist list:--- dist element:%d (%d,%d E:%d)\n", __FILE__, __LINE__, element->dist, element->x, element->y, element->enterfrom);
list->push_back(*element);
}
/****************************************************************************
* FindWay: will be in two phases to decide where to go and how
@ -572,8 +614,9 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf
fd_end.x = fd_start.x = -1;
fd_end.y = fd_start.y = -1;
fd_end.enterfrom = fd_start.enterfrom = -1;
fd_end.dist = fd_start.dist = 0;
if (FindReference(&fd_start.x, &fd_start.y, blockstart.substr(2, string::npos))) {
if (FindReference(&fd_start.x, &fd_start.y, blockstart.substr(2, string::npos),0)) {
debug (0, "Railway::FindWay found startblock (%s).", blockstart.c_str());
if (Get(fd_start.x, fd_start.y).dir == 1) {
if (blockstart[0] == '+') fd_start.enterfrom = 0;
@ -593,7 +636,7 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf
return 0;
}
if (FindReference(&fd_end.x, &fd_end.y, blockend.substr(2, string::npos))) {
if (FindReference(&fd_end.x, &fd_end.y, blockend.substr(2, string::npos), 0)) {
debug (0, "Railway::FindWay found endblock (%s).", blockend.c_str());
if (Get(fd_end.x, fd_end.y).dir == 1) {
if (blockend[0] == '+') fd_end.enterfrom = 0;
@ -620,8 +663,13 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf
// loop through the map to find all possible ways to the destination
// this will be done in respect of lockedby blocks and ways.
//
// DebugPrintFindWay(fd_data);
while ((fd_list.size() > 0 || fd_pos.enterfrom >= 0) && found == 0) {
// DebugPrintFindWay(fd_data);
// std::list<struct s_findway_data>::iterator iter;
// for (iter = fd_list.begin(); iter != fd_list.end(); iter++)
// printf ("List: Dist:%-4d Pos (%d,%d)\n", iter->dist, iter->x, iter->y);
// DebugPrintFindWay(fd_data);
if (fd_pos.enterfrom < 0) {
std::list<struct s_findway_data>::iterator iter;
@ -629,6 +677,7 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf
if (iter != fd_list.end()) {
fd_pos = *iter;
fd_list.pop_front();
// printf ("%s:%d pop_front -> fd_pos (%d,%d e:%d) dist:%d\n", __FILE__, __LINE__, fd_pos.x, fd_pos.y, fd_pos.enterfrom, fd_pos.dist);
}
}
@ -680,23 +729,24 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf
else if (rpos->type == RAILWAY_TURNOUT) {
fd_tmp = NextPos(fd_pos, rpos->altdir);
fd_tmp.way += (string)",t:" + (string)rpos->name + (string)":1";
fd_list.push_back(fd_tmp);
fd_pos = NextPos(fd_pos, rpos->dir);
if (!_NextPosIsValid(lockedfor, locoflags, &fd_pos)) {
fd_pos.enterfrom = -1;
fd_pos.x = -1;
fd_pos.y = -1;
fd_pos.way = "";
}
else fd_pos.way += (string)",t:" + (string)rpos->name + (string)":0";
if (fd_tmp.enterfrom != -1) FDListPush(&fd_list, &fd_tmp);
fd_tmp = NextPos(fd_pos, rpos->dir);
fd_tmp.way += (string)",t:" + (string)rpos->name + (string)":0";
if (fd_tmp.enterfrom != -1) FDListPush(&fd_list, &fd_tmp);
fd_pos.enterfrom = -1;
fd_pos.x = -1;
fd_pos.y = -1;
fd_pos.way = "";
}
else if (rpos->type == RAILWAY_CONNECTOR) {
int conector_found = 0;
x = -1;
y = -1;
while (conector_found == 0 && _FindReference(&x, &y, rpos->name) == 1) {
int cnt = 0;
while (conector_found == 0 && _FindReference(&x, &y, rpos->name, (cnt++)) == 1) {
if (fd_pos.x != x || fd_pos.y != y) {
fd_pos.oldx = fd_pos.x;
fd_pos.oldy = fd_pos.y;
@ -783,7 +833,13 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf
else // nothing found?
*next = "";
if (!FindWayCheckIfNoDoubleTurnouts (next)) *next = "";
//
// check that a way is not set twice
//
if (!FindWayCheckIfNoDoubleTurnouts (next)) {
debug (0, "FindWayCheckIfNoDoubleTurnouts way [%s] is not valid.", next->c_str());
*next = "";
}
}
UnLock();
@ -798,16 +854,21 @@ int Railways::FindWay(string org_blockstart, string org_blockend, string lockedf
int Railways::FindWayCheckIfNoDoubleTurnouts (string *way) {
size_t pos1, pos2; // block positions
size_t tp1, tp2; // turnouts
int wayset;
string s; // search string (should not been found)
for (pos1 = 0, pos2 = way->find(",b:"); pos2 != string::npos; pos1 = pos2, pos2 = way->find(",b:", pos1+1)) {
tp1 = pos1;
for (tp1 = way->find(",t:", tp1+1); tp1 != string::npos; tp1 = way->find(",t:", tp1+1)) {
tp2 = way->find (way->substr(tp1, way->find(":", tp1+4)-tp1), tp1+1);
if ((*way)[way->find(":", tp1+4)+1] == '0')
s = way->substr(tp1, way->find(":", tp1+4)-tp1) + ":1";
else
s = way->substr(tp1, way->find(":", tp1+4)-tp1) + ":0";
tp2 = way->find (s, tp1+4);
if (tp2 == string::npos) continue; // not found
if (tp2 < pos2) return 0; // found ... return 0
}
}
return 1;
}
@ -880,8 +941,8 @@ int Railways::_SplitBlockIsFreeAndAllowed(string loconame, int locoflags, string
if (server->blocks.GetLockedby(pblock).length() != 0) return 0;
// check the way between the blocks
if (_FindReference(&x1, &y1, pblock) == 0) return 0;
if (_FindReference(&x2, &y2, nblock) == 0) return 0;
if (_FindReference(&x1, &y1, pblock, 0) == 0) return 0;
if (_FindReference(&x2, &y2, nblock, 0) == 0) return 0;
debug (0, "Railway::_SplitBlockIsFreeAndAllowed p pos(%d,%d) n pos(%d,%d)", x1, y1, x2, y2);
x = x1; y = y1;
@ -981,7 +1042,7 @@ int Railways::LockWay (string way, string lockedby, int lockonoff) {
//
// retrieve final start position, in case of split blocks
if (FindReference(&start.x, &start.y, startblockfinal)) {
if (FindReference(&start.x, &start.y, startblockfinal, 0)) {
if (Get(start.x, start.y).dir == 1) {
if (startblock[0] == '+') start.enterfrom = 0;
else start.enterfrom = 2;
@ -1027,7 +1088,7 @@ int Railways::LockWay (string way, string lockedby, int lockonoff) {
}
// get the final end position
if (!FindReference(&end.x, &end.y, endblockfinal)) {
if (!FindReference(&end.x, &end.y, endblockfinal, 0)) {
debug (0, "Railway::FindWay could not find endblock (%s).", endblockfinal.c_str());
return 0;
}
@ -1163,11 +1224,11 @@ int Railways::LockWay (string way, string lockedby, int lockonoff) {
else if (r->type == RAILWAY_CONNECTOR) {
int found = 0;
int x, y;
int x, y, cnt;
printf ("%s:%d connector found Reference:'%s'\n", __FILE__, __LINE__, r->name);
x = -1;
y = -1;
while (found == 0 && _FindReference(&x, &y, r->name) == 1) {
while (found == 0 && _FindReference(&x, &y, r->name, (cnt++)) == 1) {
if (pos.x != x || pos.y != y) {
printf ("%s:%d found %d,%d\n", __FILE__, __LINE__, x, y);
pos.oldx = pos.x;

@ -74,6 +74,7 @@ struct s_findway_data {
int oldx;
int oldy;
int oldenterfrom;
int dist;
string way;
};
@ -98,7 +99,7 @@ class Railways {
JSONParse _GetJSONRailway(int x, int y);
void _New (int w, int h);
void DebugPrintFindWay(struct s_findway_map *fw);
int _FindReference(int *x, int *y, string name);
int _FindReference(int *x, int *y, string name, int cnt);
int _SplitBlockIsFreeAndAllowed(string loconame, int locoflags, string block);
int _NextPosIsValid(string loconame, int locoflags, struct s_findway_data *fwd);
string GetDestBlock(int locoflags, string blockend);
@ -127,7 +128,7 @@ class Railways {
void GetJSONAll(JSONParse *json);
Railway GetRailwayFromJSON(JSONParse *j);
int FindReference(int *x, int *y, string name);
int FindReference(int *x, int *y, string name, int cnt);
int FindWay(string blockstart, string blockend, string lockedfor, string *next);
int FindWayCheckIfNoDoubleTurnouts (string *next);
int FindRandomWay(string blockstart, string lockedfor, string *next);

@ -62,12 +62,13 @@ Server::~Server() {
int Server::TurnoutSet(string name, int active) {
int x, y, locked = 0;
string lockedby;
int cnt = 0;
Railway r;
//
// check if locked
while (server->railways.FindReference(&x, &y, name)) {
while (server->railways.FindReference(&x, &y, name, (cnt++))) {
r = server->railways.Get(x, y);
// printf ("%s:%d Reference %d,%d Name:%s Lockedby:%s\n", __FILE__, __LINE__, x, y, r.name, r.lockedby);
debug (0, "%s:%d Reference %d,%d Name:%s Lockedby:%s", __FILE__, __LINE__, x, y, r.name, r.lockedby);
if (r.lockedby[0] != 0) {
locked = 1;
lockedby = r.lockedby;

Loading…
Cancel
Save