From a276d8a687596fc8ac707b11ea0dd3a0c7417a38 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?C=C3=A9sar=20Benito=20Lamata?= Date: Fri, 23 Sep 2022 16:08:43 +0200 Subject: [PATCH] General fixes. Work in progress STM-DMI implementation --- .gitignore | 3 +- CMakeLists.txt | 2 +- DMI/STM/stm_objects.cpp | 170 +++++++++++ DMI/graphics/color.h | 4 + DMI/graphics/layout.cpp | 4 + DMI/graphics/layout.h | 1 + DMI/graphics/sdl/component.cpp | 2 +- DMI/init.cpp | 21 +- DMI/messages/messages.cpp | 2 +- DMI/speed.txt | 1 + DMI/speed/gauge.cpp | 7 +- DMI/tcp/server.cpp | 70 +++-- EVC/CMakeLists.txt | 2 +- EVC/DMI/dmi.cpp | 3 +- EVC/DMI/windows.cpp | 52 +++- EVC/Euroradio/terminal.cpp | 5 +- EVC/LX/level_crossing.cpp | 16 +- EVC/LX/level_crossing.h | 5 +- EVC/NationalFN/asfa.cpp | 30 +- EVC/OR_interface/interface.cpp | 72 ++++- EVC/Packets/information.h | 4 +- EVC/Packets/messages.cpp | 10 - EVC/Packets/packets.h | 20 +- EVC/Packets/radio.cpp | 13 +- EVC/Packets/radio.h | 9 +- EVC/Packets/types.h | 61 ++-- EVC/Packets/variables.h | 2 +- EVC/Position/linking.cpp | 1 + EVC/Procedures/level_transition.cpp | 89 +++++- EVC/Procedures/level_transition.h | 54 +--- EVC/Procedures/mode_transition.cpp | 10 +- EVC/Procedures/mode_transition.h | 2 + EVC/Procedures/train_trip.cpp | 13 + EVC/STM/stm.cpp | 422 ++++++++++++++++++++++++++++ EVC/STM/stm.h | 84 +++++- EVC/Supervision/national_values.cpp | 14 +- EVC/Supervision/speed_profile.cpp | 11 +- EVC/Supervision/speed_profile.h | 2 + EVC/Supervision/targets.cpp | 19 +- EVC/TrainSubsystems/brake.cpp | 15 +- EVC/TrainSubsystems/power.cpp | 9 + EVC/evc.cpp | 3 + 42 files changed, 1115 insertions(+), 224 deletions(-) create mode 100644 DMI/STM/stm_objects.cpp create mode 100644 DMI/speed.txt create mode 100644 EVC/STM/stm.cpp diff --git a/.gitignore b/.gitignore index 0b5f41cc..6803004d 100755 --- a/.gitignore +++ b/.gitignore @@ -7,7 +7,6 @@ ETCS.kdev4 .directory Thumbs.db -EVC/orts error.log MakeLists.txt.user CMakeCache.txt @@ -20,3 +19,5 @@ install_manifest.txt compile_commands.json CTestTestfile.cmake _deps +libs/ +build/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index e19d4235..8d30ae32 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required (VERSION 3.14) project (ETCS) -option(ETCS_VENDORED "Use vendored libraries" ON) +option(ETCS_VENDORED "Use vendored libraries" OFF) if (ETCS_VENDORED) set(SDL2TTF_VENDORED ON CACHE BOOL "Vendored TTF libs") add_subdirectory(libs/SDL EXCLUDE_FROM_ALL) diff --git a/DMI/STM/stm_objects.cpp b/DMI/STM/stm_objects.cpp new file mode 100644 index 00000000..8cba6c39 --- /dev/null +++ b/DMI/STM/stm_objects.cpp @@ -0,0 +1,170 @@ +#include "../graphics/button.h" +#include "../graphics/component.h" +#include "../graphics/color.h" +#include "../messages/messages.h" +#include "../window/window.h" +std::map> but_pos; +std::map> ind_pos; +void setup_areas() +{ + areas["A1"] = {0, 15, 54, 54}; + areas["A2"] = {0, 69, 54, 30}; + areas["A3"] = {0, 99, 54, 191}; + areas["A4"] = {0, 290, 54, 25}; + areas["B3"] = {140, 271, 36, 36}; + areas["B4"] = {176, 271, 36, 36}; + areas["B5"] = {212, 271, 36, 36}; + areas["C8"] = {0, 315, 54, 25}; + areas["C9"] = {0, 340, 54, 25}; + areas["C2"] = {54, 315, 37, 50}; + areas["C3"] = {91, 315, 37, 50}; + areas["C4"] = {128, 315, 37, 50}; + areas["C1"] = {165, 315, 58, 50}; + areas["C5"] = {223, 315, 37, 50}; + areas["C6"] = {260, 315, 37, 50}; + areas["C7"] = {297, 315, 37, 50}; + for (int i=1; i<=4; i++) { + areas["E"+std::to_string(i)] = {0, 365 + 20*(i-1), 54, 25}; + } + for (int i=5; i<=9; i++) { + areas["E"+std::to_string(i)] = {54, 365 + 20*(i-5), 234, 20}; + } + areas["E10"] = {288, 365, 46, 50}; + areas["E11"] = {288, 415, 46, 50}; + for (int i=5; i<=9; i++) { + areas["E"+std::to_string(i)] = {54, 365 + 20*(i-5), 234, 20}; + } + for (int i=1; i<=9; i++) { + areas["F"+std::to_string(i)] = {580, 15 + 50*(i-1), 60, 50}; + } + for (int i=1; i<=10; i++) { + areas["G"+std::to_string(i)] = {334 + ((i-1)%5)*49, 315 + ((i-1)/5) * 50, (i==5 || i==10) ? 50 : 49, 50}; + } + areas["G11"] = {334, 415, 63, 50}; + areas["G12"] = {397, 415, 120, 50}; + areas["G13"] = {517, 415, 63, 50}; +} +class ntc_window : window +{ + int nid_stm; + std::map buttons; + std::map indicators; + std::map icons; + std::map messages; + texture *get_icon(int num) + { + if (icons.find(num) == icons.end()) { + icons[num] = Component::getImage("symbols/STM/"+std::to_string(nid_stm)+"/"+std::to_string(num)); + } + return icons[num]; + } + Color get_color(int col, bool bg) + { + switch (col) { + case 0: + return bg ? DarkBlue : Black; + case 1: + return White; + case 2: + return Red; + case 3: + return Blue; + case 4: + return Green; + case 5: + return Yellow; + case 5: + return LightRed; + case 5: + return LightGreen; + } + } + public: + ntc_window() : window(construct_main) + { + + } + void display_button(int id, int icon, std::string text, int properties) + { + bool displayed = (properties>>9)&1; + if (!displayed) { + auto it = buttons.find(id); + if (it != buttons.end()) { + delete it->second; + buttons.erase(it); + } + return; + } + Button *b = new TextButton(text); + if (icon > 0) + b->add(get_icon(icon)); + bool counterflash = (properties>>8)&1; + int flash = (properties>>6)&3; + b->setBackgroundColor(get_color((properties>>3)&7, true)); + b->setForegroundColor(get_color(properties&7, false)); + buttons[id] = b; + } + void display_indicator(int id, int position, int icon, std::string text, int properties) + { + bool displayed = (properties>>9)&1; + if (!displayed) { + auto it = indicators.find(id); + if (it != indicators.end()) { + delete it->second; + indicators.erase(it); + } + return; + } + std::string area; + if (position < 4) + area = "B"+std::to_string(position+2); + else if (position == 4) + area = "H1"; + else if (position < 10) + area = "C"+std::to_string(position-3); + else if (position < 20) + area = "G"+std::to_string(position-9); + /*{ + if (position < 3) + area = "F"+std::to_string(position+7); + else if (position < 8) + area = "C"+std::to_string(position-1); + else if (position < 18) + area = "G"+std::to_string(position-7); + }*/ + if (areas.find(area) == areas.end()) + return; + auto pos = areas[area]; + Component *c = new Component(pos[2],pos[3]); + Color bg = get_color((properties>>3)&7, true); + Color fg = get_color(properties&7, false); + c->setBackgroundColor(bg); + c->setForegroundColor(fg); + if (icon > 0) + c->add(get_icon(icon)); + if (text.size() > 0) + c->addText(text, 0, 0, 12, fg); + bool counterflash = (properties>>8)&1; + int flash = (properties>>6)&3; + indicators[id] = c; + addToLayout(c, new RelativeAlignment(nullptr, pos[0], pos[1])); + } + ~ntc_window() + { + for (auto it : buttons) { + delete it.second; + } + for (auto it : indicators) { + delete it.second; + } + for (auto it : icons) { + delete it.second; + } + for (auto it : icons) { + delete it.second; + } + for (auto it : messages) { + revokeMessage(it.second.Id); + } + } +}; \ No newline at end of file diff --git a/DMI/graphics/color.h b/DMI/graphics/color.h index 12184f96..8963bd7d 100755 --- a/DMI/graphics/color.h +++ b/DMI/graphics/color.h @@ -43,4 +43,8 @@ const Color Orange = {234,145,0}; const Color DarkBlue = {3, 17, 34}; const Color PASPdark = {33, 49, 74}; const Color PASPlight = {41, 74, 107}; +const Color Blue = {0, 0, 234}; +const Color Green = {0, 234, 0}; +const Color LightRed = {255, 96, 96}; +const Color LightGreen = {96, 255, 96}; #endif \ No newline at end of file diff --git a/DMI/graphics/layout.cpp b/DMI/graphics/layout.cpp index a2434cac..9af160d1 100755 --- a/DMI/graphics/layout.cpp +++ b/DMI/graphics/layout.cpp @@ -84,4 +84,8 @@ void Layout::updateLocations() vector& Layout::getElements() { return elements; +} +Layout::~Layout() +{ + removeAll(); } \ No newline at end of file diff --git a/DMI/graphics/layout.h b/DMI/graphics/layout.h index dc5f45e2..719f9afd 100755 --- a/DMI/graphics/layout.h +++ b/DMI/graphics/layout.h @@ -60,6 +60,7 @@ class Layout { std::vector elements; public: + ~Layout(); std::vector order; void add(Component *comp, ComponentAlignment *alignment); void removeAll(); diff --git a/DMI/graphics/sdl/component.cpp b/DMI/graphics/sdl/component.cpp index 7a8cf90c..3eb70a4c 100755 --- a/DMI/graphics/sdl/component.cpp +++ b/DMI/graphics/sdl/component.cpp @@ -287,7 +287,7 @@ image_graphic *Component::getImage(string path, float cx, float cy, float sx, fl ig->cy = cy; ig->sx = sx; ig->sy = sy; - ig->load_function = [this, ig]{getImageGraphic(ig,ig->path,ig->cx, ig->cy, ig->sx, ig->sy);}; + ig->load_function = [ig,this]{getImageGraphic(ig,ig->path,ig->cx, ig->cy, ig->sx, ig->sy);}; return ig; } void Component::getImageGraphic(texture *t, string path, float cx, float cy, float sx, float sy) diff --git a/DMI/init.cpp b/DMI/init.cpp index 02789ea3..d2598733 100755 --- a/DMI/init.cpp +++ b/DMI/init.cpp @@ -32,6 +32,8 @@ void quit() #elif defined(_WIN32) #include #include +#include +#include LONG WINAPI windows_exception_handler(EXCEPTION_POINTERS * ExceptionInfo) { HANDLE process = GetCurrentProcess(); @@ -76,6 +78,9 @@ LONG WINAPI windows_exception_handler(EXCEPTION_POINTERS * ExceptionInfo) stackframe.AddrStack.Mode = AddrModeFlat; #endif*/ + MODULEINFO moduleInfo; + GetModuleInformation(process, GetModuleHandleA(NULL), &moduleInfo, sizeof(MODULEINFO)); + FILE *file = fopen("error.log", "w+"); for (size_t i = 0; i < 25; i++) { @@ -93,9 +98,17 @@ LONG WINAPI windows_exception_handler(EXCEPTION_POINTERS * ExceptionInfo) DWORD64 displacement = 0; if (SymFromAddr(process, stackframe.AddrPC.Offset, &displacement, symbol)) { - fprintf(file, "[%i] %s\n", i, symbol->Name); + fprintf(file, "[%i] %s", i, symbol->Name); + IMAGEHLP_LINE64 line; + line.SizeOfStruct = sizeof(IMAGEHLP_LINE64); + DWORD d; + if (SymGetLineFromAddr64(process, stackframe.AddrPC.Offset, &d, &line)) { + fprintf(file, " %s:%i\n", line.FileName, line.LineNumber); + } else { + fprintf(file, "\n"); + } } else { - fprintf(file, "[%i] %p\n", i, (void*)stackframe.AddrPC.Offset); + fprintf(file, "[%i] %p\n", i, (void*)(stackframe.AddrPC.Offset-(unsigned long long)moduleInfo.lpBaseOfDll+0x140000000ULL)); } } @@ -150,6 +163,7 @@ extern "C" void Java_com_etcs_dmi_DMI_DMIstop(JNIEnv *env, jobject thiz) running = false; } #endif +#include int main(int argc, char** argv) { #ifdef __ANDROID__ @@ -169,6 +183,9 @@ int main(int argc, char** argv) setSupervision(NoS); //std::thread video(init_video); std::thread tcp(startSocket); + extern int maxSpeed; + std::ifstream file("speed.txt"); + file>>maxSpeed; startWindows(); init_video(); //manage_windows(); diff --git a/DMI/messages/messages.cpp b/DMI/messages/messages.cpp index 4262fe00..0ac621c8 100755 --- a/DMI/messages/messages.cpp +++ b/DMI/messages/messages.cpp @@ -108,7 +108,7 @@ void updateMessages() textArea.setAck([msg]() { msg->ack = false; setAck(AckType::Message, msg->Id, false); - write_command("json", R"({"DriverSelection":"MessageAcknowledge,"MessageId:")"+to_string(msg->Id)+"}"); + write_command("json", R"({"DriverSelection":"MessageAcknowledge","MessageId":)"+to_string(msg->Id)+"}"); updateMessages(); }); } diff --git a/DMI/speed.txt b/DMI/speed.txt new file mode 100644 index 00000000..d411bb7c --- /dev/null +++ b/DMI/speed.txt @@ -0,0 +1 @@ +400 diff --git a/DMI/speed/gauge.cpp b/DMI/speed/gauge.cpp index 291fff06..ca8e06bb 100755 --- a/DMI/speed/gauge.cpp +++ b/DMI/speed/gauge.cpp @@ -202,13 +202,14 @@ static bool inited = false; void displayLines() { setColor(White); - for(int i = 0; i<=maxSpeed; i+=10) + int step = maxSpeed == 150 ? 5 : 10; + for(int i = 0; i<=maxSpeed; i+=step) { float size; float an = speedToAngle(i); - int longinterval = maxSpeed == 400 ? 50 : 20; + int longinterval = maxSpeed == 400 ? 50 : (maxSpeed == 150 ? 25 : 20); size = i%longinterval!=0 ? -110 : -100; - if(!inited && ((maxSpeed != 400 && i%20==0) || (maxSpeed == 400 && i%50==0 && i!=250 && i!=350))) + if(!inited && i%longinterval == 0 && (maxSpeed != 400 || (i!=250 && i!=350))) { std::string s = to_string(i); const char *str = s.c_str(); diff --git a/DMI/tcp/server.cpp b/DMI/tcp/server.cpp index 30a19c43..2fca83f8 100755 --- a/DMI/tcp/server.cpp +++ b/DMI/tcp/server.cpp @@ -40,7 +40,8 @@ #include "../control/control.h" #include int server; -int client; +int clients[3]; +int active_channel; #define BUFF_SIZE 1024 #define PORT 5010 static char data[BUFF_SIZE]; @@ -310,9 +311,9 @@ void parseData(string str) notifyDataReceived(); } extern bool running; -void read() +int read(int channel) { - int result = recv(client, ::data, BUFF_SIZE-1, 0); + int result = recv(clients[channel], ::data, BUFF_SIZE-1, 0); if(result>0) { ::data[result] = 0; @@ -325,12 +326,40 @@ void read() buffer = buffer.substr(end+1); } } - if (result < 0) running = false; + return result; } void write_command(string command, string value) { + if (active_channel < 0) + return; string tosend = command+"("+value+");\n"; - send(client, tosend.c_str(), tosend.size(), 0); + send(clients[active_channel], tosend.c_str(), tosend.size(), 0); +} +void listenChannels() +{ + while(active_channel == -1 && running) + { + struct sockaddr_in addr; + int c = sizeof(struct sockaddr_in); + int cl = accept(server, (struct sockaddr *)&addr, +#ifdef _WIN32 + (int *) +#else + (socklen_t *) +#endif + &c); + if(cl == -1) { + perror("accept"); + continue; + } + for (int i=0; i<3; i++) { + if (clients[i] < 0) { + clients[i] = cl; + active_channel = i; + break; + } + } + } } void startSocket() { @@ -351,31 +380,28 @@ void startSocket() perror("bind"); return; } - if (listen(server, 1) == -1) { + if (listen(server, 3) == -1) { perror("listen"); return; } - client = -1; - while(client == -1 && running) + active_channel = -1; + for (int i=0; i<3; i++) + clients[i] = -1; + listenChannels(); + while(running) { - struct sockaddr_in addr; - int c = sizeof(struct sockaddr_in); - int cl = accept(server, (struct sockaddr *)&addr, + int res = read(active_channel); + if (res < 0) + { #ifdef _WIN32 - (int *) + closesocket(clients[active_channel]); #else - (socklen_t *) + close(clients[active_channel]); #endif - &c); - if(cl == -1) { - perror("accept"); - continue; + clients[active_channel] = -1; + active_channel = -1; + if (running) listenChannels(); } - client = cl; - } - while(running) - { - read(); } closeSocket(); } diff --git a/EVC/CMakeLists.txt b/EVC/CMakeLists.txt index f641e93f..06dd767b 100644 --- a/EVC/CMakeLists.txt +++ b/EVC/CMakeLists.txt @@ -5,7 +5,7 @@ Supervision/emergency_stop.cpp Supervision/acceleration.cpp antenna.cpp MA/movement_authority.cpp MA/mode_profile.cpp Position/linking.cpp OR_interface/interface.cpp SSP/ssp.cpp Packets/packets.cpp Procedures/mode_transition.cpp LX/level_crossing.cpp Packets/messages.cpp Packets/radio.cpp Packets/vbc.cpp Euroradio/session.cpp Euroradio/terminal.cpp -Packets/logging.cpp +Packets/logging.cpp STM/stm.cpp Procedures/start.cpp Procedures/override.cpp Procedures/train_trip.cpp Procedures/level_transition.cpp Procedures/stored_information.cpp TrackConditions/track_conditions.cpp Time/clock.cpp Position/geographical.cpp DMI/text_messages.cpp DMI/windows.cpp DMI/track_ahead_free.cpp TrainSubsystems/power.cpp TrainSubsystems/brake.cpp diff --git a/EVC/DMI/dmi.cpp b/EVC/DMI/dmi.cpp index 8d14880c..fb134b48 100644 --- a/EVC/DMI/dmi.cpp +++ b/EVC/DMI/dmi.cpp @@ -213,7 +213,6 @@ void to_json(json&j, const text_message &t) j["FirstGroup"] = t.firstGroup; j["Acknowledge"] = t.ack; } -static json j; void dmi_comm() { fd = socket(AF_INET, SOCK_STREAM, 0); @@ -231,6 +230,7 @@ void dmi_comm() unique_lock lck(loop_mtx); sendtoor = get_milliseconds() - lastor > 250; if (sendtoor) lastor = get_milliseconds(); + json j; j["AllowedSpeedMpS"] = V_perm; j["InterventionSpeedMpS"] = V_sbi; j["TargetSpeedMpS"] = V_target; @@ -374,7 +374,6 @@ void dmi_comm() j["SpeedTargets"] = "[]"_json; j["ActiveTrackConditions"] = "[]"_json; } - send_command("json", j.dump()); /* send_command("setVset", to_string(V_set*3.6)); diff --git a/EVC/DMI/windows.cpp b/EVC/DMI/windows.cpp index 87a25c82..13588979 100644 --- a/EVC/DMI/windows.cpp +++ b/EVC/DMI/windows.cpp @@ -7,6 +7,7 @@ #include "../Procedures/train_trip.h" #include "../Procedures/level_transition.h" #include "../Packets/vbc.h" +#include "../STM/stm.h" #include dialog_sequence active_dialog; std::string active_dialog_step; @@ -67,13 +68,15 @@ json level_window() case Level::N3: lv = "Level 3"; break; - case Level::NTC: - switch(nid_ntc) { - case 0: - lv = "ASFA"; - break; + case Level::NTC: { + auto it = ntc_names.find(nid_ntc); + if (it != ntc_names.end()) { + lv = it->second; + } else { + lv = "NTC "+std::to_string(nid_ntc); } break; + } } j["level"] = lv; std::vector levels; @@ -92,17 +95,27 @@ json level_window() case Level::N3: levels.push_back("Level 3"); break; - case Level::NTC: - switch(lti.nid_ntc) { - case 0: - levels.push_back("ASFA"); - break; + case Level::NTC: { + auto it = ntc_names.find(lti.nid_ntc); + if (it != ntc_names.end()) { + levels.push_back(it->second); + } else { + levels.push_back("NTC "+std::to_string(lti.nid_ntc)); } break; + } } } } else { - levels = {"Level 0", "ASFA", "Level 1", "Level 2"}; + levels = {"Level 0", "Level 1", "Level 2"}; + for (auto &kvp : installed_stms) { + auto it = ntc_names.find(kvp.first); + if (it != ntc_names.end()) { + levels.push_back(it->second); + } else { + levels.push_back("NTC "+std::to_string(kvp.first)); + } + } } j["Levels"] = levels; return j; @@ -254,6 +267,13 @@ void update_dmi_windows() active_dialog_step = "D2"; else active_dialog_step = "S1"; + if (train_data_valid && som_active) { + for (auto kvp : installed_stms) { + auto *stm = kvp.second; + if (!stm->isolated && stm->state != stm_state::CO && stm->state != stm_state::CS && stm->state != stm_state::HS && stm->state != stm_state::DA) + send_failed_msg(stm); + } + } } else if (active_dialog_step == "D2") { if (supervising_rbc && supervising_rbc->status == session_status::Established) { active_dialog_step = "S9"; @@ -626,7 +646,15 @@ void update_dialog_step(std::string step, std::string step2) trigger_condition(19); } if (V_est == 0 && (level==Level::N0 || level==Level::NTC || level==Level::N1) && mode != Mode ::SH) { - trigger_condition(5); + if (level == Level::NTC) { + auto *stm = get_stm(nid_ntc); + if (stm != nullptr && stm->national_trip) + trigger_condition(35); + else + trigger_condition(5); + } else { + trigger_condition(5); + } } if (V_est == 0 && (level == Level::N2 || level == Level::N3)) { if (supervising_rbc && supervising_rbc->status == session_status::Established && emergency_stops.empty()) { diff --git a/EVC/Euroradio/terminal.cpp b/EVC/Euroradio/terminal.cpp index 1ad1a0b4..a9d3b7fd 100644 --- a/EVC/Euroradio/terminal.cpp +++ b/EVC/Euroradio/terminal.cpp @@ -49,10 +49,7 @@ bool mobile_terminal::setup(communication_session *session) lck.unlock(); bit_manipulator w; msg->write_to(w); - int size = (w.bits.size()+7)/8; - unsigned char buff[size]; - w.get_bytes(buff); - int result = ::send(fd, (char*)buff, size, 0); + int result = ::send(fd, (char*)&w.bits[0], w.bits.size(), 0); if (result < 0) { released--; if (status == safe_radio_status::Connected) diff --git a/EVC/LX/level_crossing.cpp b/EVC/LX/level_crossing.cpp index 5440c6b3..b5a277e9 100644 --- a/EVC/LX/level_crossing.cpp +++ b/EVC/LX/level_crossing.cpp @@ -25,8 +25,12 @@ void load_lx(LevelCrossingInformation lxi, distance ref) { level_crossing lx = level_crossing(lxi, ref); for (auto it=level_crossings.begin(); it!=level_crossings.end(); ) { - if (it->id == lx.id) it = level_crossings.erase(it); - else ++it; + if (it->id == lx.id) { + lx.svl_replaced = it->svl_replaced; + it = level_crossings.erase(it); + } else { + ++it; + } } level_crossings.insert(lx); calculate_SvL(); @@ -41,11 +45,10 @@ void update_lx() } if (it->svl_replaced && d_minsafefront(it->start) < it->start+it->length) inform_lx = true; if (V_est == 0 && it->stop && d_estfront > it->start-it->stoplength) { - it->svl_replaced = true; - it->svl_replaced_loc = d_estfront_dir[odometer_orientation == -1]; + it->svl_replaced = d_estfront_dir[odometer_orientation == -1]; calculate_SvL(); } - if (EoA && *EoA == it->start && !it->svl_replaced) { + if (EoA && *EoA == it->start && !it->svl_replaced) { const std::list &supervised_targets = get_supervised_targets(); const target *tEoA, *tSvL; for (auto it = supervised_targets.begin(); it != supervised_targets.end(); ++it) { @@ -64,8 +67,7 @@ void update_lx() bool c1 = tEoA->d_P < d_estfront; bool c2 = tSvL->d_P < d_maxsafefront(tSvL->d_P); if (c1 || c2) { - it->svl_replaced = true; - it->svl_replaced_loc = c1 ? d_estfront : d_maxsafefront(tSvL->d_P); + it->svl_replaced = c1 ? d_estfront : d_maxsafefront(tSvL->d_P); calculate_SvL(); } } diff --git a/EVC/LX/level_crossing.h b/EVC/LX/level_crossing.h index b952f707..ef4fac2a 100644 --- a/EVC/LX/level_crossing.h +++ b/EVC/LX/level_crossing.h @@ -18,6 +18,7 @@ #pragma once #include "../Position/distance.h" #include "../Packets/88.h" +#include "../optional.h" #include class level_crossing { @@ -29,8 +30,7 @@ class level_crossing double V_LX; bool stop; double stoplength; - mutable bool svl_replaced; - mutable distance svl_replaced_loc; + mutable optional svl_replaced; level_crossing(LevelCrossingInformation lx, distance ref) { id = lx.NID_LX; @@ -43,7 +43,6 @@ class level_crossing stop = lx.Q_STOPLX == Q_STOPLX_t::StopRequired; if (stop) stoplength = lx.L_STOPLX.get_value(lx.Q_SCALE); } - svl_replaced = false; } bool operator<(const level_crossing l) const { diff --git a/EVC/NationalFN/asfa.cpp b/EVC/NationalFN/asfa.cpp index 22b183b3..ece73148 100644 --- a/EVC/NationalFN/asfa.cpp +++ b/EVC/NationalFN/asfa.cpp @@ -34,11 +34,13 @@ extern mutex iface_mtx; bool detected = false; bool connected = false; bool active = false; +bool brake_commanded = false; void register_parameter(std::string parameter); void initialize_asfa() { std::unique_lock lck(iface_mtx); - Parameter *p = new Parameter("asfa::akt"); + Parameter *p; + /*p = new Parameter("asfa::akt"); p->GetValue = []() { return AKT ? "1" : "0"; }; @@ -48,7 +50,7 @@ void initialize_asfa() p->GetValue = []() { return CON ? "1" : "0"; }; - manager.AddParameter(p); + manager.AddParameter(p);*/ p = new Parameter("asfa::conectado"); p->SetValue = [](string val) { @@ -65,17 +67,18 @@ extern double V_NVUNFIT; extern optional UN_speed; int64_t akt_delay=0; int64_t con_delay=0; -void send_msg() +text_message &send_msg() { bool con = CON; std::string nivel=""; - if (level == Level::N0 || CON) nivel = "Nivel 0 - "; + if (level == Level::N0 || (ongoing_transition && ongoing_transition->leveldata.level == Level::N0)) nivel = "Nivel 0 - "; + else if ((level == Level::NTC && nid_ntc == 0) || (ongoing_transition && ongoing_transition->leveldata.level == Level::NTC && ongoing_transition->leveldata.nid_ntc == 0)) nivel = ""; else if (level == Level::N1) nivel = "Nivel 1 - "; else if (level == Level::N2) nivel = "Nivel 2 - "; else if (level == Level::N3) nivel = "Nivel 3 - "; int64_t time = get_milliseconds(); - add_message(text_message(nivel + (connected ? "ASFA conectado en C.G." : "ASFA anulado en C.G."), false, false, false, [time,con](text_message &m) { - return time+30000leveldata.level == Level::N0 || (ongoing_transition->leveldata.level == Level::NTC && ongoing_transition->leveldata.nid_ntc == 0))) { if (!CON) { CON = true; diff --git a/EVC/OR_interface/interface.cpp b/EVC/OR_interface/interface.cpp index 9d9daa5b..9d922b9b 100644 --- a/EVC/OR_interface/interface.cpp +++ b/EVC/OR_interface/interface.cpp @@ -27,6 +27,7 @@ #include "../Position/linking.h" #include "../Packets/messages.h" #include "../TrainSubsystems/power.h" +#include "../STM/stm.h" #include #include #include @@ -73,7 +74,7 @@ void SetParameters() std::unique_lock lck(iface_mtx); Parameter *p = new Parameter("distance"); p->SetValue = [](string val) { - or_dist = stof(val); + or_dist = stod(val); if (odometer_value > or_dist) odometer_direction = -1; else if (odometer_value < or_dist) @@ -86,7 +87,7 @@ void SetParameters() p = new Parameter("speed"); p->SetValue = [](string val) { double prev = V_est; - V_est = stof(val)/3.6; + V_est = stod(val)/3.6; V_ura = 0.007*V_est; if (V_est < 0.2) { @@ -99,7 +100,7 @@ void SetParameters() p = new Parameter("acceleration"); p->SetValue = [](string val) { - A_est = stof(val); + A_est = stod(val); }; manager.AddParameter(p); @@ -124,9 +125,10 @@ void SetParameters() p = new Parameter("etcs::telegram"); p->SetValue = [](string val) { - std::vector message; + std::vector message((val.size()+7)>>3); for (int i=0; i>3] |= 1<<(7-(i&7)); } bit_manipulator r(std::move(message)); eurobalise_telegram t(r); @@ -173,13 +175,15 @@ void SetParameters() extern MonitoringStatus monitoring; const target *t = (monitoring == CSM) ? indication_target : &MRDT; if (t != nullptr) { + //t->calculate_curves(); double speed = t->get_target_speed(); double dist; if (t->type == target_class::EoA || t->type == target_class::SvL) dist = std::max(std::min(*EoA-d_estfront, *SvL-d_maxsafefront(*SvL)), 0.0); else - dist = std::max(t->d_P-d_maxsafefront(t->get_target_position()), 0.0); - return std::to_string(V_perm)+";"+std::to_string(speed)+";"+std::to_string(dist); + dist = std::max(t->get_target_position()-d_maxsafefront(t->get_target_position()), 0.0);//std::max(t->d_P-d_maxsafefront(t->get_target_position()), 0.0);*/ + float atf = std::max(speed, std::min(sqrt(2*0.4f*(dist-V_est*10-10) + speed * speed)-3/3.6, V_perm)); + return std::to_string(atf); } else return std::to_string(V_perm); }; @@ -187,7 +191,7 @@ void SetParameters() p = new Parameter("cruise_speed"); p->SetValue = [](string val) { - V_set = stof(val)/3.6; + V_set = stod(val)/3.6; }; manager.AddParameter(p); @@ -239,6 +243,57 @@ void SetParameters() p->SetValue = [](string val) { parse_command(val, false); }; + + p = new Parameter("stm::asfa::override"); + p->SetValue = [](string val) { + if (installed_stms.find(0) != installed_stms.end()) + installed_stms[0]->report_override(); + }; + manager.AddParameter(p); + + p = new Parameter("stm::asfa::trip"); + p->SetValue = [](string val) { + if (installed_stms.find(0) != installed_stms.end()) + installed_stms[0]->report_trip(); + }; + manager.AddParameter(p); + + p = new Parameter("stm::asfa::state"); + p->SetValue = [](string val) { + if (installed_stms.find(0) == installed_stms.end()) { + installed_stms[0] = new stm_object(); + installed_stms[0]->nid_stm = 0; + } + installed_stms[0]->report_received((stm_state)stoi(val)); + }; + manager.AddParameter(p); + + p = new Parameter("stm::asfa::request"); + p->SetValue = [](string val) { + if (installed_stms.find(0) != installed_stms.end()) { + installed_stms[0]->request_state((stm_state)stoi(val)); + } + }; + manager.AddParameter(p); + + p = new Parameter("stm::asfa::order"); + p->GetValue = []() { + string s = ""; + if (installed_stms.find(0) != installed_stms.end()) { + auto *asfa = installed_stms[0]; + if (asfa->last_order) { + s = std::to_string((int)asfa->last_order->state)+","+(asfa->last_order->conditional?"1":"0"); + } + } + return s; + }; + manager.AddParameter(p); + + p = new Parameter("stm::override"); + p->GetValue = []() { + extern bool overrideProcedure; + return overrideProcedure ? "1" : "0"; + }; manager.AddParameter(p); } void register_parameter(string parameter) @@ -280,6 +335,7 @@ void start_or_iface() s_client->WriteLine("register(master_key)"); s_client->WriteLine("register(controller::direction)"); s_client->WriteLine("register(train_orientation)"); + s_client->WriteLine("register(stm::asfa::*)"); SetParameters(); thread t(polling); t.detach(); diff --git a/EVC/Packets/information.h b/EVC/Packets/information.h index 17082d1a..57f1935b 100644 --- a/EVC/Packets/information.h +++ b/EVC/Packets/information.h @@ -80,8 +80,10 @@ struct signalling_information : etcs_information { Level1_MA ma = *(Level1_MA*)linked_packets.front().get(); if (ma.V_MAIN == 0) { - if (!overrideProcedure && !infill) + if (!overrideProcedure && !infill) { trigger_condition(18); + trigger_condition(67); + } } else { movement_authority newMA = movement_authority(ref, ma, timestamp); set_signalling_restriction(newMA, infill); diff --git a/EVC/Packets/messages.cpp b/EVC/Packets/messages.cpp index da53b42b..debddd93 100644 --- a/EVC/Packets/messages.cpp +++ b/EVC/Packets/messages.cpp @@ -59,7 +59,6 @@ static distance bg_reference; static distance bg_referencemax; static distance bg_referencemin; static bool stop_checking_linking=false; -std::list::iterator link_expected=linking.end(); std::deque>> pending_telegrams; optional rams_reposition_mitigation; void trigger_reaction(int reaction); @@ -900,10 +899,6 @@ bool level_filter(std::shared_ptr info, std::listleveldata.level != Level::NTC/* || ongoing_transition->leveldata.nid_ntc != STM_id*/) - return false; - } return true; } else { if (s.exceptions.find(1) != s.exceptions.end()) { @@ -916,11 +911,6 @@ bool level_filter(std::shared_ptr info, std::listleveldata.level == Level::NTC/* && ongoing_transition->leveldata.nid_ntc == STM_id*/) - transition_buffer.back().push_back(info); - return false; - } return false; } return false; diff --git a/EVC/Packets/packets.h b/EVC/Packets/packets.h index f3e73b29..fd4d5a1e 100644 --- a/EVC/Packets/packets.h +++ b/EVC/Packets/packets.h @@ -24,6 +24,7 @@ struct ETCS_message bool valid; bool readerror; virtual void write_to(bit_manipulator &b) = 0; + virtual ~ETCS_message() {} }; struct ETCS_packet { @@ -43,12 +44,11 @@ struct ETCS_packet } virtual void write_to(bit_manipulator &w) { - int start = w.bits.size(); + int start = w.position; copy(w); - L_PACKET.rawdata = w.bits.size()-start; + L_PACKET.rawdata = w.position-start; w.replace(&L_PACKET, start+8); } - //static std::map packet_factory; static ETCS_packet *construct(bit_manipulator &r); }; struct ETCS_nondirectional_packet : ETCS_packet @@ -65,26 +65,18 @@ struct ETCS_directional_packet : ETCS_packet { directional = true; } - /*ETCS_directional_packet(bit_manipulator &r) - { - directional = true; - r.read(&NID_PACKET); - r.read(&Q_DIR); - r.read(&L_PACKET); - r.position += L_PACKET - L_PACKET.size - Q_DIR.size - NID_PACKET.size; - }*/ virtual void copy(bit_manipulator &w) override { - std::cout<<"TODO: serialize() not implemented for packet "< base64_decode(std::string str, bool remove_padding); std::shared_ptr euroradio_message::build(bit_manipulator &r) { - int size = r.bits.size()/8; + int size = r.bits.size(); NID_MESSAGE_t nid; r.peek(&nid); euroradio_message* msg; - switch ((unsigned char)nid) - { + switch (nid.rawdata) { case 2: msg = new SR_authorisation(); break; case 3: msg = new MA_message(); break; case 6: msg = new TR_exit_recognition(); break; @@ -57,7 +56,7 @@ std::shared_ptr euroradio_message::build(bit_manipulator &r) default: r.sparefound = true; msg = new euroradio_message(); break; } msg->copy(r); - while (!r.error && r.position<=r.bits.size()-8) + while (!r.error && r.position<=(r.bits.size()*8-8)) { NID_PACKET_t NID_PACKET; r.peek(&NID_PACKET); @@ -73,11 +72,11 @@ std::shared_ptr euroradio_message::build(bit_manipulator &r) } std::shared_ptr euroradio_message_traintotrack::build(bit_manipulator &r) { - int size = r.bits.size()/8; + int size = r.bits.size(); NID_MESSAGE_t nid; r.peek(&nid); euroradio_message_traintotrack *msg; - switch ((unsigned char)nid) { + switch (nid.rawdata) { case 129: msg = new validated_train_data_message(); break; case 130: msg = new SH_request(); break; case 132: msg = new MA_request(); break; @@ -86,7 +85,7 @@ std::shared_ptr euroradio_message_traintotrack:: default: r.sparefound = true; msg = new euroradio_message_traintotrack(); break; } msg->copy(r); - while (!r.error && r.position<=r.bits.size()-8) + while (!r.error && r.position<=(r.bits.size()*8-8)) { NID_PACKET_t NID_PACKET; r.peek(&NID_PACKET); diff --git a/EVC/Packets/radio.h b/EVC/Packets/radio.h index 18163c53..09fbf99e 100644 --- a/EVC/Packets/radio.h +++ b/EVC/Packets/radio.h @@ -63,9 +63,7 @@ struct euroradio_message : public ETCS_message for (auto pack : optional_packets) { pack->write_to(w); } - while(w.bits.size()%8) w.bits.push_back(false); - L_MESSAGE_t L_MESSAGE; - L_MESSAGE.rawdata = w.bits.size()/8; + L_MESSAGE.rawdata = w.bits.size(); w.replace(&L_MESSAGE, 8); w.log_entries[1].second = std::to_string(L_MESSAGE.rawdata); } @@ -84,7 +82,6 @@ struct euroradio_message_traintotrack : public ETCS_message std::vector> optional_packets; euroradio_message_traintotrack() { - L_MESSAGE.rawdata = 0; } euroradio_message_traintotrack(bit_manipulator &r) { @@ -128,9 +125,7 @@ struct euroradio_message_traintotrack : public ETCS_message for (auto pack : optional_packets) { pack->write_to(w); } - while(w.bits.size()%8) w.bits.push_back(false); - L_MESSAGE_t L_MESSAGE; - L_MESSAGE.rawdata = w.bits.size()/8; + L_MESSAGE.rawdata = w.bits.size(); w.replace(&L_MESSAGE, 8); w.log_entries[1].second = std::to_string(L_MESSAGE.rawdata); } diff --git a/EVC/Packets/types.h b/EVC/Packets/types.h index da58378b..2449de9c 100644 --- a/EVC/Packets/types.h +++ b/EVC/Packets/types.h @@ -21,31 +21,32 @@ #include #include #include +#include template class ETCS_variable_custom; struct bit_manipulator { - std::vector bits; + std::vector bits; std::vector> log_entries; bool write_mode; int position; bool error=false; bool sparefound=false; - bit_manipulator() + bit_manipulator() : position(0) { write_mode = true; } - bit_manipulator(std::vector &&bits) : bits(bits), position(0) + bit_manipulator(std::vector &&bits) : bits(bits), position(0) { write_mode = false; } bit_manipulator(unsigned char *data, int count) : position(0) { write_mode = false; - for (int i=0; i=0; j--) { - bits.push_back(((data[i]>>j) & 1)); - } - } + bits.insert(bits.end(), data, data+count); + } + inline bool operator[](int pos) + { + return bits[pos>>3] & (1<<(7-(pos&7))); } template void log(ETCS_variable_custom *var) @@ -61,11 +62,11 @@ struct bit_manipulator T value=0; int count=var->size; while(count-->0) { - if (bits.size() <= position) { + if ((bits.size()<<3) <= position) { error = true; return; } - value = value<<1 | bits[position++]; + value = value<<1 | operator[](position++); } var->rawdata = value; if (!var->is_valid()) @@ -76,13 +77,13 @@ struct bit_manipulator void peek(ETCS_variable_custom *var, int offset=0) { int position = this->position+offset; - uint32_t value=0; + T value=0; int count=var->size; while(count-->0) { - if (bits.size() <= position) { + if ((bits.size()<<3) <= position) { return; } - value = value<<1 | bits[position++]; + value = value<<1 | operator[](position++); } var->rawdata = value; } @@ -92,35 +93,25 @@ struct bit_manipulator T value=var->rawdata; int count=var->size; while(count-->0) { - bits.push_back((value>>count)&1); + bool bit = (value>>count)&1; + if ((position & 7) == 0) + bits.push_back(bit<<7); + else if (bit) + bits[position>>3] |= 1<<(7 - (position&7)); + ++position; } log(var); } template void replace(ETCS_variable_custom *var, int pos) { - if (var->size + pos > bits.size()) return; + if (var->size + pos > (bits.size()<<3)) return; for (int i=0; isize; i++) { - bits[pos+i] = (var->rawdata>>(var->size-i-1)) & 1; - } - } - void get_bytes(unsigned char *data) - { - int div = bits.size()/8; - int rem = bits.size()%8; - for (int i=0; i 0) { - char c = 0; - for (int i=0; i>3; + bits[b] ^= ((bits[b]>>off)&1)<rawdata>>(var->size-i-1)) & 1)< invalid; - ETCS_variable_custom(int size) : size(size){} + ETCS_variable_custom(int size) : size(size), rawdata(T(0)) {} operator T() const { return rawdata; diff --git a/EVC/Position/linking.cpp b/EVC/Position/linking.cpp index d3109316..255d66d5 100644 --- a/EVC/Position/linking.cpp +++ b/EVC/Position/linking.cpp @@ -21,6 +21,7 @@ #include "../Packets/messages.h" #include "../TrainSubsystems/cold_movement.h" std::list linking; +std::list::iterator link_expected = linking.end(); std::list lrbgs; bool position_valid=false; void load_train_position() diff --git a/EVC/Procedures/level_transition.cpp b/EVC/Procedures/level_transition.cpp index c95a0aea..7bf22b31 100644 --- a/EVC/Procedures/level_transition.cpp +++ b/EVC/Procedures/level_transition.cpp @@ -21,13 +21,14 @@ #include "../DMI/text_message.h" #include "../TrainSubsystems/brake.h" #include "../TrainSubsystems/cold_movement.h" +#include "../STM/stm.h" optional ongoing_transition; optional sh_transition; std::vector priority_levels; bool priority_levels_valid = false; optional transition_border; Level level = Level::Unknown; -int nid_ntc; +int nid_ntc = -1; bool level_valid = false; std::list>> transition_buffer; bool level_acknowledgeable = false; @@ -36,6 +37,7 @@ Level level_to_ack; int ntc_to_ack; bool level_timer_started = false; int64_t level_timer; +std::map ntc_names; #include void load_level() { @@ -54,10 +56,15 @@ void save_level() } void driver_set_level(level_information li) { + stm_level_change(li, true); level_valid = li.level != Level::Unknown; position_report_reasons[6] = (li.level != Level::N2 && li.level != Level::N3 && (level == Level::N2 || level == Level::N3)) ? 2 : 1; level = li.level; - nid_ntc = li.nid_ntc; + if (level == Level::NTC) { + nid_ntc = li.nid_ntc; + } else { + nid_ntc = -1; + } save_level(); } void perform_transition() @@ -65,6 +72,7 @@ void perform_transition() if(!ongoing_transition) return; level_transition_information lti = *ongoing_transition; + stm_level_change({lti.leveldata.level, lti.leveldata.nid_ntc}, false); if (level == Level::N2 || level == Level::N3) transition_border = lti.start; else @@ -72,11 +80,16 @@ void perform_transition() priority_levels = lti.priority_table; priority_levels_valid = true; ongoing_transition = {}; + STM_max_speed = {}; + STM_system_speed = {}; if (level_to_ack == Level::NTC || level == Level::NTC || level_to_ack == Level::N0) level_acknowledgeable = !level_acknowledged; Level prevlevel = level; level = lti.leveldata.level; - nid_ntc = lti.leveldata.nid_ntc; + if (level == Level::NTC) + nid_ntc = lti.leveldata.nid_ntc; + else + nid_ntc = -1; for (auto it=transition_buffer.begin(); it!=transition_buffer.end(); ++it) { for (auto it2 = it->begin(); it2!=it->end(); ++it2) { try_handle_information(*it2, *it); @@ -122,14 +135,20 @@ void update_level_status() } void level_transition_received(level_transition_information info) { - if (!ongoing_transition || ongoing_transition->leveldata.level != info.leveldata.level) + stm_level_transition_received(info); + if (!ongoing_transition || ongoing_transition->leveldata.level != info.leveldata.level || (ongoing_transition->leveldata.level == Level::NTC && ongoing_transition->leveldata.nid_ntc == info.leveldata.nid_ntc)) { level_acknowledged = false; + STM_max_speed = {}; + STM_system_speed = {}; + } level_acknowledgeable = false; level_timer_started = false; transition_buffer.clear(); transition_buffer.push_back({}); if (info.leveldata.level == level && (level != Level::NTC || info.leveldata.nid_ntc == nid_ntc)) { ongoing_transition = {}; + STM_max_speed = {}; + STM_system_speed = {}; priority_levels = info.priority_table; priority_levels_valid = true; save_level(); @@ -139,6 +158,8 @@ void level_transition_received(level_transition_information info) sh_transition = info; priority_levels = info.priority_table; ongoing_transition = {}; + STM_max_speed = {}; + STM_system_speed = {}; priority_levels_valid = true; save_level(); return; @@ -148,4 +169,64 @@ void level_transition_received(level_transition_information info) ntc_to_ack = ongoing_transition->leveldata.nid_ntc; if (ongoing_transition->immediate) perform_transition(); +} +level_transition_information::level_transition_information(LevelTransitionOrder o, distance ref) +{ + if (o.D_LEVELTR == D_LEVELTR_t::Now) { + immediate = true; + start = ref; + } else { + immediate = false; + start = ref+o.D_LEVELTR.get_value(o.Q_SCALE); + } + std::vector priorities; + priorities.push_back({start-o.element.L_ACKLEVELTR.get_value(o.Q_SCALE), o.element.M_LEVELTR.get_level(),(int)o.element.NID_NTC}); + for (int i=0; i priorities; + priorities.push_back({start, o.element.M_LEVELTR.get_level(), (int)o.element.NID_NTC}); + for (int i=0; i &priorities) +{ + for (auto &p : priorities) { + if (p.level == Level::NTC) { + assign_stm(p.nid_ntc, false); + auto *stm = get_stm(p.nid_ntc); + if (stm != nullptr && stm->available()) { + leveldata = p; + return; + } + } + if (p.level == Level::N2) { + for (auto &t : mobile_terminals) { + leveldata = p; + return; + } + } + if (p.level == Level::N0 || p.level == Level::N1) { + leveldata = p; + return; + } + } + leveldata = priorities.back(); } \ No newline at end of file diff --git a/EVC/Procedures/level_transition.h b/EVC/Procedures/level_transition.h index 84fb63e2..0e43ed44 100644 --- a/EVC/Procedures/level_transition.h +++ b/EVC/Procedures/level_transition.h @@ -44,54 +44,9 @@ struct level_transition_information distance start; target_level_information leveldata; std::vector priority_table; - level_transition_information(LevelTransitionOrder o, distance ref) - { - if (o.D_LEVELTR == D_LEVELTR_t::Now) { - immediate = true; - start = ref; - } else { - immediate = false; - start = ref+o.D_LEVELTR.get_value(o.Q_SCALE); - } - std::vector priorities; - priorities.push_back({start-o.element.L_ACKLEVELTR.get_value(o.Q_SCALE), o.element.M_LEVELTR.get_level(),(int)o.element.NID_NTC}); - for (int i=0; i priorities; - priorities.push_back({start, o.element.M_LEVELTR.get_level(), (int)o.element.NID_NTC}); - for (int i=0; i &priorities); }; void update_level_status(); void level_transition_received(level_transition_information info); @@ -105,4 +60,5 @@ extern std::list>> transition_buffer extern bool level_acknowledgeable; extern bool level_acknowledged; extern Level level_to_ack; -extern int ntc_to_ack; \ No newline at end of file +extern int ntc_to_ack; +extern std::map ntc_names; \ No newline at end of file diff --git a/EVC/Procedures/mode_transition.cpp b/EVC/Procedures/mode_transition.cpp index 25904e87..48410779 100644 --- a/EVC/Procedures/mode_transition.cpp +++ b/EVC/Procedures/mode_transition.cpp @@ -29,9 +29,9 @@ #include "../LX/level_crossing.h" #include cond mode_conditions[75]; -static std::vector transitions; static std::vector ordered_transitions[20]; Mode mode=Mode::SB; +int64_t last_mode_change; bool mode_acknowledgeable=false; bool mode_acknowledged=false; Mode mode_to_ack; @@ -89,9 +89,12 @@ void initialize_mode_transitions() c[68] = [](){return (level==Level::N0 || level==Level::NTC) && V_est==0 && !train_data_valid && mode_acknowledged;}; c[69] = [](){return get_SSP().empty() || get_SSP().begin()->get_start()>d_estfront || get_gradient().empty() || get_gradient().begin()->first>d_estfront;}; c[70] = [](){return mode_to_ack==Mode::LS && mode_acknowledged;}; + c[71] = [](){return !mode_profiles.empty() && mode_profiles.front().mode == Mode::LS && mode_profiles.front().start < d_maxsafefront(mode_profiles.front().start) && (level == Level::N1 || level == Level::N2 || level==Level::N3);}; + c[72] = [](){return !mode_profiles.empty() && mode_profiles.front().mode == Mode::LS && mode_profiles.front().start < d_maxsafefront(mode_profiles.front().start);}; c[73] = [](){return !(in_mode_ack_area && mode_to_ack == Mode::LS) && !mode_profiles.empty() && mode_profiles.front().mode == Mode::OS && mode_profiles.front().start < d_maxsafefront(mode_profiles.front().start);}; c[74] = [](){return !(in_mode_ack_area && mode_to_ack == Mode::OS) && !mode_profiles.empty() && mode_profiles.front().mode == Mode::LS && mode_profiles.front().start < d_maxsafefront(mode_profiles.front().start);}; + std::vector transitions; transitions.push_back({Mode::SB, Mode::SR, {8,37}, 7}); transitions.push_back({Mode::FS, Mode::SR, {37}, 6}); transitions.push_back({Mode::LS, Mode::SR, {37}, 6}); @@ -218,6 +221,8 @@ void update_mode_status() lrbg.dir = 1-lrbg.dir; lrbg.position = distance(lrbg.position.get(), odometer_orientation, 0); } + void reset_eurobalise_data(); + reset_eurobalise_data(); } prev_desk_open = desk_open; update_mode_profile(); @@ -241,6 +246,7 @@ void update_mode_status() if (transition != mode_to_ack) mode_acknowledgeable = false; Mode prevmode = mode; mode = transition; + last_mode_change = get_milliseconds(); if (mode == Mode::TR || mode == Mode::LS || mode == Mode::OS || mode == Mode::SH) overrideProcedure = false; if (mode == Mode::SR) { @@ -386,6 +392,8 @@ void set_mode_deleted_data() information_list[3].delete_info = []() {delete_MA();}; information_list[4].delete_info = []() {delete_gradient();}; information_list[5].delete_info = []() {delete_SSP();}; + information_list[7].delete_info = []() {STM_max_speed = {};}; + information_list[8].delete_info = []() {STM_system_speed = {};}; information_list[9].delete_info = []() {ongoing_transition = {}; transition_buffer.clear(); sh_transition = {};}; information_list[18].delete_info = []() {signal_speeds.clear();}; information_list[22].delete_info = []() { diff --git a/EVC/Procedures/mode_transition.h b/EVC/Procedures/mode_transition.h index eb7ef935..ee86dc7d 100644 --- a/EVC/Procedures/mode_transition.h +++ b/EVC/Procedures/mode_transition.h @@ -28,6 +28,8 @@ extern bool mode_acknowledgeable; extern bool mode_acknowledged; extern Mode mode_to_ack; +extern int64_t last_mode_change; +extern bool desk_open; class cond { bool triggered; diff --git a/EVC/Procedures/train_trip.cpp b/EVC/Procedures/train_trip.cpp index 0ea3316f..83e3d248 100644 --- a/EVC/Procedures/train_trip.cpp +++ b/EVC/Procedures/train_trip.cpp @@ -18,6 +18,7 @@ #include "train_trip.h" #include "../MA/movement_authority.h" #include "mode_transition.h" +#include "../STM/stm.h" #include bool trip_acknowledged = false; bool trip_exit_acknowledged = false; @@ -65,6 +66,18 @@ void train_trip(int reason) case 69: str = "No track description"; break; + case 38: { + std::string name = "NTC"; + for (auto &kvp : installed_stms) { + if (kvp.second->national_trip) { + auto it = ntc_names.find(kvp.first); + if (it != ntc_names.end()) + name = it->second; + } + } + str = name+" brake demand"; + break; + } default: break; } diff --git a/EVC/STM/stm.cpp b/EVC/STM/stm.cpp new file mode 100644 index 00000000..5cea36e6 --- /dev/null +++ b/EVC/STM/stm.cpp @@ -0,0 +1,422 @@ +#include "stm.h" +#include "../Supervision/speed_profile.h" +#include "../Procedures/override.h" +#include "../Packets/messages.h" +#include "../Packets/etcs_information.h" +#include "../Supervision/train_data.h" +std::map installed_stms; +std::map ntc_to_stm; +std::map> ntc_to_stm_lookup_table; +bool stm_control_EB = false; +bool ntc_unavailable_msg = false; +struct stm_transition +{ + stm_state from; + stm_state to; + std::set conditions; + std::string happens(stm_object *stm) + { + std::string cond = ""; + for (auto c : conditions) { + if (stm->conditions[c]()) + cond = c; + } + return cond; + } + stm_transition(stm_state from, stm_state to, std::set conditionnum) : from(from), to(to), conditions(conditionnum) + { + + } +}; +static std::map> ordered_transitions; +void stm_object::trigger_condition(std::string change) { + auto &available = ordered_transitions[(int)state]; + for (auto &t : available) { + if (t.conditions.find(change) != t.conditions.end()) { + conditions[change].trigger(); + break; + } + } +} +void stm_object::request_state(stm_state req) +{ + if (req == stm_state::CO) + trigger_condition("A2"); + else if (req == stm_state::DE && train_data_valid) + trigger_condition("A3"); + else if (req == stm_state::CS) + trigger_condition("A4a"); + else if (req == stm_state::FA && state != stm_state::FA) + send_failed_msg(this); + auto &available = ordered_transitions[(int)state]; + bool allowed = req == state; + for (auto &t : available) { + if (t.to == req) + allowed = true; + } + if (!allowed) + trigger_condition("A16"); +} +stm_object::stm_object() +{ + commands = stm_commands(); + state = stm_state::NP; + last_order = {}; + last_national_trip = 0; + national_trip = isolated = false; + conditions["A1"] = [this] {return state == stm_state::PO;}; + conditions["E4a"] = [] {return mode == Mode::SB;}; + conditions["G4a"] = [this] { + if (state == stm_state::HS && !overrideProcedure) { + if (ongoing_transition && ongoing_transition->leveldata.level == Level::NTC && get_stm(ongoing_transition->leveldata.nid_ntc) == this) { + return false; + } + if (level == Level::NTC && get_stm(nid_ntc) == this) { + return false; + } + return true; + } + return false; + }; + conditions["H4a"] = []{return mode == Mode::SB && !desk_open;}; + conditions["I4a"] = []{return mode == Mode::SH;}; + conditions["L4a"] = []{return mode == Mode::TR;}; + conditions["A6"] = [this]{ + if (state == stm_state::CS) { + for (auto kvp : installed_stms) { + auto *stm = kvp.second; + if (stm != this && stm->state == stm_state::HS) + return false; + } + if (ongoing_transition && ongoing_transition->leveldata.level == Level::NTC && get_stm(ongoing_transition->leveldata.nid_ntc) == this) { + return true; + } + } + return false; + }; + conditions["B6"] = [this]{ + if (mode == Mode::SB && desk_open && state == stm_state::CS && level == Level::NTC && level_valid) { + for (auto kvp : installed_stms) { + auto *stm = kvp.second; + if (stm != this && stm->state == stm_state::HS) + return false; + } + if (get_stm(nid_ntc) == this) + return true; + } + return false; + }; + conditions["A9"] = [this] { + if ((state == stm_state::CS || state == stm_state::HS) && level == Level::NTC && (mode == Mode::SN || mode == Mode::SL || mode == Mode::NL)) { + for (auto kvp : installed_stms) { + auto *stm = kvp.second; + if (stm != this && stm->state == stm_state::DA) + return false; + } + if (get_stm(nid_ntc) == this) + return true; + } + return false; + }; + conditions["C16"] = [this] {return last_order && last_order->state != stm_state::DA && (last_order->state != stm_state::CS || !last_order->conditional) && get_milliseconds() - last_order_time > 10000;}; + conditions["D16"] = [this] {return last_order && last_order->state == stm_state::DA && get_milliseconds() - last_order_time > 5000;}; + conditions["E16"] = [this] {return last_order && last_order->state == stm_state::CS && last_order->conditional && !national_trip && get_milliseconds() - last_order_time > 5000;}; +} +void fill_stm_transitions() +{ + std::vector stm_transitions; + stm_transitions.push_back({stm_state::NP, stm_state::PO, {"A1"}}); + stm_transitions.push_back({stm_state::PO, stm_state::CO, {"A2"}}); + stm_transitions.push_back({stm_state::CO, stm_state::DE, {"A3"}}); + stm_transitions.push_back({stm_state::CO, stm_state::CS, {"A4a"}}); + stm_transitions.push_back({stm_state::DE, stm_state::CS, {"A4a"}}); + stm_transitions.push_back({stm_state::CS, stm_state::HS, {"A6", "B6"}}); + stm_transitions.push_back({stm_state::CS, stm_state::DA, {"A9"}}); + stm_transitions.push_back({stm_state::HS, stm_state::DA, {"A9"}}); + stm_transitions.push_back({stm_state::NP, stm_state::FA, {"A17", "B16"}}); + stm_transitions.push_back({stm_state::PO, stm_state::FA, {"A16", "B16", "C16", "H16", "I16", "L16", "P16", "A17"}}); + stm_transitions.push_back({stm_state::CO, stm_state::FA, {"A16", "B16", "C16", "H16", "I16", "L16", "P16", "A17"}}); + stm_transitions.push_back({stm_state::DE, stm_state::FA, {"A16", "B16", "C16", "H16", "I16", "L16", "P16", "A17"}}); + stm_transitions.push_back({stm_state::CS, stm_state::FA, {"A16", "B16", "C16", "D16", "H16", "N16", "O16", "P16", "A17"}}); + stm_transitions.push_back({stm_state::HS, stm_state::FA, {"A16", "B16", "C16", "D16", "H16", "N16", "O16", "P16", "A17"}}); + stm_transitions.push_back({stm_state::DA, stm_state::FA, {"A16", "B16", "C16", "E16", "F16", "H16", "N16", "O16", "P16", "Q16", "A17"}}); + stm_transitions.push_back({stm_state::FA, stm_state::NP, {"A15"}}); + stm_transitions.push_back({stm_state::FA, stm_state::PO, {"A1"}}); + stm_transitions.push_back({stm_state::DA, stm_state::CS, {"B4a", "B4b", "I4a", "A4b", "E4a", "K4a", "L4a"}}); + stm_transitions.push_back({stm_state::HS, stm_state::CS, {"C4a", "E4b", "G4a", "H4b", "I4a", "J4a"}}); + + for (auto &trans : stm_transitions) { + ordered_transitions[(int)trans.from].push_back(trans); + } +} +void update_ntc_transitions() +{ + for (auto kvp : installed_stms) { + auto *stm = kvp.second; + auto &available = ordered_transitions[(int)stm->state]; + for (stm_transition &t : available) { + if (t.to != stm_state::FA && stm->last_order) + continue; + std::string type = t.happens(stm); + if (type != "") { + if (t.to != stm_state::NP && t.to != stm_state::PO && type != "A17") { + if (t.to == stm_state::CS) { + bool conditional = type == "A4b" || type == "B4b"; + stm->last_order = {stm_state::CS, conditional}; + } else { + stm->last_order = {t.to, false}; + } + stm->last_order_time = get_milliseconds(); + std::cout<<"STM: "<state = stm_state::FA; + send_failed_msg(stm); + } + } + if (t.to == stm_state::PO) + stm->state = stm_state::PO; + break; + } + } + } +} +void assign_stm(int nid_ntc, bool driver) +{ + if (ntc_to_stm.find(nid_ntc) == ntc_to_stm.end()) { + int nid_stm = -1; + if (driver) { + auto it = ntc_to_stm_lookup_table.find(nid_ntc); + std::vector table; + if (it != ntc_to_stm_lookup_table.end()) + table = it->second; + if (table.empty()) { + nid_stm = nid_ntc; + } else { + for (auto *stm : table) { + if (stm->available()) { + nid_stm = stm->nid_stm; + break; + } + } + if (nid_stm < 0) { + nid_stm = (*table.begin())->nid_stm; + } + if (nid_stm < 0) { + nid_stm = (*table.begin())->nid_stm; + } + } + } else { + auto it = ntc_to_stm_lookup_table.find(nid_ntc); + std::vector table; + if (it != ntc_to_stm_lookup_table.end()) + table = it->second; + if (table.empty()) { + nid_stm = nid_ntc; + } else { + for (auto *stm : table) { + if (stm->available()) { + nid_stm = stm->nid_stm; + break; + } + } + if (nid_stm < 0) { + for (auto *stm : table) { + if (stm->state != stm_state::FA && !stm->isolated) { + nid_stm = stm->nid_stm; + break; + } + } + } + if (nid_stm < 0) { + nid_stm = (*table.begin())->nid_stm; + } + } + } + if (nid_stm >= 0) + ntc_to_stm[nid_ntc] = nid_stm; + } +} +void stm_level_change(level_information newlevel, bool driver) +{ + if (newlevel.level == Level::NTC && driver) + assign_stm(newlevel.nid_ntc, true); + if (level == Level::NTC) { + auto *stm1 = get_stm(nid_ntc); + if (stm1 != nullptr) { + if (newlevel.level == Level::NTC) { + auto *stm2 = get_stm(newlevel.nid_ntc); + if (stm1 != stm2) + stm1->trigger_condition(driver ? "B4b" : "A4b"); + } else { + stm1->trigger_condition(driver ? "K4a" : "B4a"); + if (stm1->national_trip) + mode_conditions[38].trigger(); + } + } + } + if (newlevel.level == Level::NTC && !driver) { + auto *stm = get_stm(newlevel.nid_ntc); + if (stm != nullptr && !stm->available()) { + stm->trigger_condition("I16"); + } + } + if (newlevel.level != Level::NTC || nid_ntc != newlevel.nid_ntc) + ntc_to_stm.erase(nid_ntc); +} +void stm_level_transition_received(level_transition_information info) +{ + if (ongoing_transition && ongoing_transition->leveldata.level == Level::NTC) { + auto *stm1 = get_stm(ongoing_transition->leveldata.nid_ntc); + if (stm1->state == stm_state::HS) { + if (info.leveldata.level == Level::NTC) { + auto stm2 = get_stm(info.leveldata.nid_ntc); + if (stm1 != stm2) { + stm1->trigger_condition("C4a"); + } + } else { + stm1->trigger_condition("J4a"); + } + } + } +} +void stm_object::report_trip() +{ + if (national_trip && (mode == Mode::PT || mode == Mode::UN)) + trigger_condition("Q16"); + last_national_trip = get_milliseconds(); +} +void stm_object::report_override() +{ + if (active()) + start_override(); +} +void stm_object::report_received(stm_state newstate) +{ + if (state != newstate) { + auto &available = ordered_transitions[(int)state]; + bool allowed = false; + for (auto &t : available) { + if (t.to == newstate) + allowed = true; + } + if (!allowed) + trigger_condition("B16"); + if (newstate == stm_state::FA) + trigger_condition("A17"); + } + + if (state != newstate && newstate == stm_state::DA) { + STM_max_speed = {}; + recalculate_MRSP(); + } + state = newstate; + if (last_order && last_order->state == state) + last_order = {}; + +} +void send_failed_msg(stm_object *stm) +{ + text_message msg(get_ntc_name(stm->nid_stm) + " failed", true, true, 2, [stm](text_message &msg){return msg.acknowledged;}); + add_message(msg); +} +bool mode_filter(std::shared_ptr info, std::list> message); +void request_STM_max_speed(stm_object *stm, double speed) +{ + if (ongoing_transition && ongoing_transition->leveldata.level == Level::NTC && ongoing_transition->leveldata.nid_ntc != nid_ntc) { + if (stm->state == stm_state::HS && stm == get_stm(ongoing_transition->leveldata.nid_ntc)) { + auto info = std::shared_ptr(new etcs_information(8)); + info->handle_fun = [speed]() { + STM_max_speed = speed_restriction(speed, ongoing_transition->start, distance(std::numeric_limits::max(), 0, 0), false); + recalculate_MRSP(); + }; + std::list> msg = {info}; + if (mode_filter(info, msg)) + info->handle(); + } + } +} +void request_STM_system_speed(stm_object *stm, double speed, double dist) +{ + if (ongoing_transition && ongoing_transition->leveldata.level == Level::NTC && level != Level::NTC) { + auto it = ntc_to_stm.find(ongoing_transition->leveldata.nid_ntc); + if (stm->state == stm_state::HS && stm == get_stm(ongoing_transition->leveldata.nid_ntc)) { + auto info = std::shared_ptr(new etcs_information(9)); + distance start = ongoing_transition->start - dist; + distance end = ongoing_transition->start; + info->handle_fun = [speed, start, end]() { + STM_system_speed = speed_restriction(speed, start, end, false); + recalculate_MRSP(); + }; + std::list> msg = {info}; + if (mode_filter(info, msg)) + info->handle(); + } + } +} +stm_object *get_stm(int nid_ntc) +{ + auto stm_it = ntc_to_stm.find(nid_ntc); + if (stm_it == ntc_to_stm.end() || installed_stms.find(stm_it->second) == installed_stms.end()) + return nullptr; + return installed_stms[stm_it->second]; +} +std::string get_ntc_name(int nid_ntc) +{ + auto it = ntc_names.find(nid_ntc); + if (it != ntc_names.end()) + return it->second; + return "NTC "+std::to_string(nid_ntc); +} +static Mode prev_mode; +void update_stm_control() +{ + if (level != Level::NTC) + nid_ntc = -1; + if (mode != prev_mode && (mode == Mode::NP || mode == Mode::SB)) + ntc_to_stm.clear(); + prev_mode = mode; + + for (auto kvp : installed_stms) { + kvp.second->national_trip = get_milliseconds() - kvp.second->last_national_trip < 10000; + } + + update_ntc_transitions(); + + auto *stm = get_stm(nid_ntc); + bool msg = false; + if (stm != nullptr && !stm->available() && !stm->isolated) { + stm->control_request_EB = true; + if (mode == Mode::SN || (mode == Mode::NL && get_milliseconds()-last_mode_change > 5000)) { + if (!ntc_unavailable_msg) { + ntc_unavailable_msg = true; + text_message msg(get_ntc_name(nid_ntc) + " not available", true, false, 2, [stm](text_message &msg){ + if (stm != get_stm(nid_ntc) || stm->available() || stm->isolated || (mode != Mode::SN && mode != Mode::NL)) { + ntc_unavailable_msg = false; + return true; + } + return false; + }); + add_message(msg); + } + } + } + stm_control_EB = false; + for (auto kvp : installed_stms) { + auto *stm2 = kvp.second; + if (stm2->state == stm_state::DA || level == Level::N0 || level == Level::N1 || level == Level::N2 || level == Level::N3 || stm != stm2 || mode != Mode::SN || stm->isolated) + stm2->control_request_EB = false; + if (stm2->last_order && stm2->last_order->conditional && stm2->last_order->state == stm_state::CS && stm2->state != stm_state::CS && (stm2->state != stm_state::FA || V_est > 0)) + stm_control_EB = true; + stm_control_EB |= stm2->control_request_EB; + } + if (ongoing_transition && ongoing_transition->leveldata.level == Level::NTC && !STM_max_speed) { + stm = get_stm(ongoing_transition->leveldata.nid_ntc); + if (stm != nullptr && !stm->available()) + request_STM_max_speed(stm, 0); + } +} +void setup_stm_control() +{ + ntc_names[0] = "ASFA"; + fill_stm_transitions(); +} \ No newline at end of file diff --git a/EVC/STM/stm.h b/EVC/STM/stm.h index 5652780a..4f9ce478 100644 --- a/EVC/STM/stm.h +++ b/EVC/STM/stm.h @@ -1,6 +1,86 @@ +#pragma once #include -class stm +#include +#include "../Procedures/mode_transition.h" +#include "../Procedures/level_transition.h" +#include "../Supervision/supervision.h" +#include "../optional.h" +enum struct stm_state +{ + NP, + PO, + CO, + DE, + CS, + HS, + DA, + FA +}; +struct stm_state_order +{ + stm_state state; + bool conditional; +}; +struct stm_commands +{ + bool TCO; + bool SB; + bool EB_on_SB_failure; + bool EB; + bool open_circuit_breaker; + bool lower_pantograph; + bool close_air_intake; + bool regenerative_brake_inhibition; + bool magnetic_shoe_inhibition; + bool eddy_emergency_brake_inhibition; + bool eddy_service_brake_inhibition; +}; +struct stm_forwarded_info +{ + int direction; + int active_cab; +}; +struct stm_object { int nid_stm; + stm_commands commands; + stm_state state; + optional last_order; + int64_t last_order_time; + int64_t last_national_trip; + bool national_trip; + bool isolated; + bool control_request_EB; + std::map conditions; + stm_object(); + bool active() + { + if (last_order) { + if (last_order->state == stm_state::DA) + return true; + if (last_order->state != stm_state::CS || !last_order->conditional) + return false; + } + return state == stm_state::DA; + } + bool available() + { + return state == stm_state::CS || state == stm_state::HS || state == stm_state::DA; + } + void report_received(stm_state newstate); + void report_override(); + void report_trip(); + void trigger_condition(std::string change); + void request_state(stm_state req); }; -std::map> ntc_to_stm; +void stm_level_change(level_information newlevel, bool driver); +void stm_level_transition_received(level_transition_information info); +void send_failed_msg(stm_object *stm); +void assign_stm(int nid_ntc, bool driver); +void setup_stm_control(); +void update_stm_control(); +stm_object *get_stm(int nid_ntc); +std::string get_ntc_name(int nid_ntc); +extern std::map installed_stms; +extern std::map ntc_to_stm; +extern bool stm_control_EB; \ No newline at end of file diff --git a/EVC/Supervision/national_values.cpp b/EVC/Supervision/national_values.cpp index 1e53806c..313cf470 100644 --- a/EVC/Supervision/national_values.cpp +++ b/EVC/Supervision/national_values.cpp @@ -214,10 +214,7 @@ void load_national_values(NationalValues nv) #endif bit_manipulator w; nv.copy(w); - int size = (w.bits.size()+7)/8; - char buff[size]; - w.get_bytes((unsigned char*)buff); - file.write(buff, size); + file.write((const char*)&w.bits[0], w.bits.size()); } struct StoredNationalValueSet { @@ -242,13 +239,11 @@ void setup_national_values() #else std::ifstream file("nationalvalues.bin", std::ios::binary); #endif - std::vector message; + std::vector message; while (!file.eof() && !file.fail()) { char c; file.read(&c, 1); - for (int i=7; i>=0; i--) { - message.push_back((c>>i)&1); - } + message.push_back(c); } bit_manipulator r(std::move(message)); NationalValues nv = NationalValues(); @@ -256,8 +251,9 @@ void setup_national_values() std::cout<<"Loading national values\n"; for (auto &var : r.log_entries) { - std::cout< train_speed; optional SR_speed; optional SH_speed; optional UN_speed; -optional STM_speed; optional OS_speed; optional LS_speed; optional override_speed; +optional STM_system_speed; +optional STM_max_speed; static std::map gradient; void delete_back_info() { @@ -107,7 +108,7 @@ void recalculate_MRSP() restrictions.insert(it->restriction); if (mode == Mode::FS || mode == Mode::OS || mode == Mode::LS) { for (auto it=level_crossings.begin(); it!=level_crossings.end(); ++it) { - if (!it->lx_protected && it->svl_replaced) restrictions.insert(speed_restriction(it->V_LX, it->svl_replaced_loc, it->start+it->length, false)); + if (!it->lx_protected && it->svl_replaced) restrictions.insert(speed_restriction(it->V_LX, *it->svl_replaced, it->start+it->length, false)); } } if (train_speed && @@ -123,8 +124,10 @@ void recalculate_MRSP() restrictions.insert(*SH_speed); if (UN_speed && mode == Mode::UN) restrictions.insert(*UN_speed); - if (STM_speed && mode == Mode::SN) - restrictions.insert(*STM_speed); + if (STM_system_speed && (mode == Mode::FS || mode == Mode::LS || mode == Mode::OS || mode == Mode::SR || mode == Mode::UN)) + restrictions.insert(*STM_system_speed); + if (STM_max_speed && (mode == Mode::FS || mode == Mode::LS || mode == Mode::OS || mode == Mode::SR || mode == Mode::UN || mode == Mode::SN)) + restrictions.insert(*STM_max_speed); if (override_speed && (mode == Mode::SH || mode == Mode::SR || mode == Mode::UN)) restrictions.insert(*override_speed); if (mode == Mode::FS || mode == Mode::OS || mode == Mode::LS) diff --git a/EVC/Supervision/speed_profile.h b/EVC/Supervision/speed_profile.h index 997ea2bb..b13c0590 100644 --- a/EVC/Supervision/speed_profile.h +++ b/EVC/Supervision/speed_profile.h @@ -91,5 +91,7 @@ extern optional SH_speed; extern optional UN_speed; extern optional OS_speed; extern optional LS_speed; +extern optional STM_system_speed; +extern optional STM_max_speed; extern optional override_speed; speed_restriction get_PBD_restriction(double d_PBD, distance start, distance end, bool EB, double g); diff --git a/EVC/Supervision/targets.cpp b/EVC/Supervision/targets.cpp index 778ca7cc..fb1ee1d8 100644 --- a/EVC/Supervision/targets.cpp +++ b/EVC/Supervision/targets.cpp @@ -141,9 +141,15 @@ void target::calculate_curves(double V_est, double A_est, double V_delta) const distance v_sbi_dappr = d_maxsafefront(d_target) + V_est*T_bs2 + D_be_display; V_SBI2 = v_sbi_dappr < get_distance_curve(V_target) ? std::max(get_speed_curve(v_sbi_dappr)-(V_delta0+V_delta1+V_delta2),V_target + dV_sbi(V_target)) : (V_target + dV_sbi(V_target)); - //GUI disabled distance v_p_dappr = d_maxsafefront(d_target) + V_est*(T_driver+T_bs2) + D_be_display; - V_P = v_p_dappr < get_distance_curve(V_target) ? std::max(get_speed_curve(v_p_dappr) - (V_delta0+V_delta1+V_delta2), V_target) : V_target; + if (v_p_dappr < get_distance_curve(V_target) || (Q_NVGUIPERM && d_maxsafefront(d_target) < get_distance_gui_curve(V_target))) { + V_P = get_speed_curve(v_p_dappr) - (V_delta0+V_delta1+V_delta2); + if (Q_NVGUIPERM) + V_P = std::min(V_P, get_speed_gui_curve(d_maxsafefront(d_target))); + V_P = std::max(V_P, V_target); + } else { + V_P = V_target; + } } else { d_SBI1 = get_distance_curve(V_est) - T_bs1*V_est; d_W = d_SBI1 - T_warning*V_est; @@ -154,9 +160,14 @@ void target::calculate_curves(double V_est, double A_est, double V_delta) const distance v_sbi_dappr = d_estfront_dir[d_target.get_orientation() == -1] + V_est*T_bs1; V_SBI1 = v_sbi_dappr < d_target ? get_speed_curve(v_sbi_dappr) : 0; - //GUI disabled distance v_p_dappr = d_estfront_dir[d_target.get_orientation() == -1] + V_est*(T_driver + T_bs1); - V_P = v_p_dappr < d_target ? get_speed_curve(v_p_dappr) : 0; + if (v_p_dappr < d_target) { + V_P = get_speed_curve(v_p_dappr); + if (Q_NVGUIPERM) + V_P = std::min(V_P, get_speed_gui_curve(d_estfront)); + } else { + V_P = 0; + } } } optional EoA; diff --git a/EVC/TrainSubsystems/brake.cpp b/EVC/TrainSubsystems/brake.cpp index 9c19495f..377ef84e 100644 --- a/EVC/TrainSubsystems/brake.cpp +++ b/EVC/TrainSubsystems/brake.cpp @@ -22,6 +22,7 @@ #include "brake.h" #include "../Procedures/stored_information.h" #include "../Euroradio/session.h" +#include "../STM/stm.h" extern bool SB; extern bool EB; bool SB_commanded; @@ -105,7 +106,17 @@ void handle_brake_command() SB_commanded = !brake_conditions.empty(); SB_commanded |= SB; EB_commanded = EB; + EB_commanded |= stm_control_EB; + if (mode == Mode::SN) { + for (auto kvp : installed_stms) { + auto *stm = kvp.second; + if (stm->active()) { + SB_commanded |= stm->commands.SB; + EB_commanded |= stm->commands.EB; + } + } + } if ((prevEB || prevSB) && !EB_commanded && !SB_commanded) send_command("playSinfo",""); - prevEB = EB; - prevSB = SB; + prevEB = EB_commanded; + prevSB = SB_commanded; } \ No newline at end of file diff --git a/EVC/TrainSubsystems/power.cpp b/EVC/TrainSubsystems/power.cpp index 209dd0c8..e5972df6 100644 --- a/EVC/TrainSubsystems/power.cpp +++ b/EVC/TrainSubsystems/power.cpp @@ -17,6 +17,7 @@ */ #include "../TrackConditions/track_condition.h" #include "power.h" +#include "../STM/stm.h" track_condition_profile_external neutral_section_info; track_condition_profile_external lower_pantograph_info; track_condition_profile_external air_tightness_info; @@ -31,4 +32,12 @@ extern bool TCO; void update_power_status() { traction_cutoff_status = !TCO; + if (mode == Mode::SL || mode == Mode::NL || mode == Mode::SN) { + for (auto kvp : installed_stms) { + auto *stm = kvp.second; + if (stm->active()) { + traction_cutoff_status &= !stm->commands.TCO; + } + } + } } \ No newline at end of file diff --git a/EVC/evc.cpp b/EVC/evc.cpp index 7d1a7026..ae9d5927 100755 --- a/EVC/evc.cpp +++ b/EVC/evc.cpp @@ -43,6 +43,7 @@ #include "TrackConditions/track_condition.h" #include "TrainSubsystems/subsystems.h" #include "LX/level_crossing.h" +#include "STM/stm.h" #include #ifdef __ANDROID__ @@ -219,6 +220,7 @@ void start() setup_national_values(); load_vbcs(); initialize_mode_transitions(); + setup_stm_control(); set_message_filters(); initialize_national_functions(); started = true; @@ -230,6 +232,7 @@ void update() update_track_comm(); update_national_values(); update_procedures(); + update_stm_control(); update_supervision(); update_lx(); update_track_conditions();