001package jmri.jmrit.logix; 002 003import java.io.File; 004import java.io.IOException; 005import java.io.FileNotFoundException; 006import java.util.ArrayList; 007import java.util.List; 008import java.util.TreeMap; 009import java.util.Map.Entry; 010 011import javax.annotation.CheckForNull; 012 013import jmri.DccLocoAddress; 014import jmri.DccThrottle; 015import jmri.InstanceManager; 016import jmri.LocoAddress; 017import jmri.LocoAddress.Protocol; 018import jmri.implementation.SignalSpeedMap; 019import jmri.jmrit.XmlFile; 020import jmri.jmrit.logix.ThrottleSetting.Command; 021import jmri.jmrit.logix.ThrottleSetting.CommandValue; 022import jmri.jmrit.logix.ThrottleSetting.ValueType; 023import jmri.jmrit.roster.Roster; 024import jmri.jmrit.roster.RosterEntry; 025import jmri.jmrit.roster.RosterSpeedProfile; 026import jmri.jmrit.roster.RosterSpeedProfile.SpeedStep; 027 028import org.jdom2.Attribute; 029import org.jdom2.Element; 030import org.jdom2.JDOMException; 031 032/** 033 * All speed related method transferred from Engineer and Warrant classes. 034 * Until June 2017, the problem of determining the actual track speed of a 035 * model train in millimeters per millisecond (same as meters/sec) from the 036 * throttle setting was usually done with an ad hoc "throttle factor". When 037 * created, the RosterSpeedProfile provides this needed conversion but 038 * generally is not done by users for each of their locos. 039 * 040 * Methods to dynamically determine a RosterSpeedProfile for each loco are 041 * implemented in this class. 042 * 043 * @author Pete Cressman Copyright (C) 2009, 2010, 2017 044 * 045 */ 046public class SpeedUtil { 047 048 private DccLocoAddress _dccAddress; 049 private String _rosterId; // Roster title for train 050 private RosterEntry _rosterEntry; 051 052 private DccThrottle _throttle; 053 private boolean _isForward = true; 054 private float _rampThrottleIncrement; // user specified throttle increment for ramping 055 private int _rampTimeIncrement; // user specified time for ramp step increment 056 057 private RosterSpeedProfile _sessionProfile; // speeds measured in the session 058 private final SignalSpeedMap _signalSpeedMap; 059 private int _ma; // milliseconds needed to increase speed by throttle step amount 060 private int _md; // milliseconds needed to decrease speed by throttle step amount 061 private ArrayList<BlockSpeedInfo> _speedInfo; // map max speeds and occupation times of each block in route 062 063 // A SCALE_FACTOR of 44.704 divided by _scale, computes a scale speed of 100mph at full throttle. 064 // This is set arbitrarily and can be modified by the Preferences "throttle Factor". 065 // Only used when there is no SpeedProfile. 066 public static final float SCALE_FACTOR = 44.704f; // divided by _scale, gives a rough approximation for track speed 067 public static final float MAX_TGV_SPEED = 88889; // maximum speed of a Bullet train (320 km/hr) in millimeters/sec 068 069 protected SpeedUtil() { 070 _signalSpeedMap = InstanceManager.getDefault(SignalSpeedMap.class); 071 } 072 073 /** 074 * @return RosterEntry 075 */ 076 @CheckForNull 077 public RosterEntry getRosterEntry() { 078 return _rosterEntry; 079 } 080 081 /** 082 * Set the key identifier for the Speed Profile 083 * If a RosterEntry exists, _rosterId is the RosterEntry id 084 * or possibly is the RosterEntrytitle. 085 * Otherwise it may be just the decoder address 086 * @return key to speedProfile 087 */ 088 public String getRosterId() { 089 return _rosterId; 090 } 091 092 /** 093 * Set a key to a loco's roster and speed info. 094 * If there is no RosterEntry, the id still locates 095 * a session SpeedProfile for the loco. 096 * Called from: 097 * SpeedUtil.setDccAdress(String) - main parser 098 * WarrantFrame.setup() - edit existing warrant 099 * WarrantManagerXml - load warrant 100 * @param id key to speedProfile 101 * @return true if RosterEntry exists for id 102 */ 103 public boolean setRosterId(String id) { 104 log.trace("setRosterId({}) old={}", id, _rosterId); 105 if (id == null || id.isEmpty()) { 106 _rosterEntry = null; 107 _sessionProfile = null; 108 return false; 109 } 110 if (id.equals(_rosterId)) { 111 return true; 112 } else { 113 _sessionProfile = null; 114 RosterEntry re = Roster.getDefault().getEntryForId(id); 115 if (re != null) { 116 _rosterEntry = re; 117 _dccAddress = re.getDccLocoAddress(); 118 _rosterId = id; 119 return true; 120 } 121 } 122 return false; 123 } 124 125 public DccLocoAddress getDccAddress() { 126 if (_dccAddress == null) { 127 if (_rosterEntry != null) { 128 _dccAddress = _rosterEntry.getDccLocoAddress(); 129 } 130 } 131 return _dccAddress; 132 } 133 134 @CheckForNull 135 protected String getAddress() { 136 if (_dccAddress == null) { 137 _dccAddress = getDccAddress(); 138 } 139 if (_dccAddress != null) { 140 return _dccAddress.toString(); 141 } 142 return null; 143 } 144 145 /** 146 * Called by: 147 * Warrant.setRunMode() about to run a warrant 148 * WarrantFrame.setup() for an existing warrant 149 * WarrantTableModel.cloneWarrant() when cloning an existing warrant 150 * 151 * @param dccAddr DccLocoAddress 152 */ 153 protected void setDccAddress(DccLocoAddress dccAddr) { 154 log.trace("setDccAddress(DccLocoAddress) _dccAddress= {}", _dccAddress); 155 if (dccAddr == null) { 156 _sessionProfile = null; 157 _rosterId = null; 158 _rosterEntry = null; 159 _dccAddress = null; 160 return; 161 } 162 if (!dccAddr.equals(_dccAddress)) { 163 _sessionProfile = null; 164 _dccAddress = dccAddr; 165 } 166 } 167 168 public boolean setDccAddress(int number, String type) { 169 log.trace("setDccAddress({}, {})", number, type); 170 LocoAddress.Protocol protocol; 171 switch (type) { 172 case "L": 173 case "l": 174 protocol = LocoAddress.Protocol.DCC_LONG; 175 break; 176 case "S": 177 case "s": 178 protocol = LocoAddress.Protocol.DCC_SHORT; 179 break; 180 default: 181 try { 182 protocol = Protocol.getByPeopleName(type); 183 } catch (IllegalArgumentException iae) { 184 try { 185 protocol = Protocol.getByShortName(type.toLowerCase()); 186 } catch (IllegalArgumentException e) { 187 _dccAddress = null; 188 return false; 189 } 190 } 191 break; 192 } 193 DccLocoAddress addr = new DccLocoAddress(number, protocol); 194 if (_rosterEntry != null && addr.equals(_rosterEntry.getDccLocoAddress())) { 195 return true; 196 } else { 197 _dccAddress = addr; 198 String numStr = String.valueOf(number); 199 List<RosterEntry> l = Roster.getDefault().matchingList(null, null, 200 numStr, null, null, null, null); 201 if (!l.isEmpty()) { 202 int size = l.size(); 203 if ( size!= 1) { 204 log.info("{} entries for address {}, {}", l.size(), number, type); 205 } 206 _rosterEntry = l.get(size - 1); 207 setRosterId(_rosterEntry.getId()); 208 } else { 209 // DCC address is set, but there is not a Roster entry for it 210 _rosterId = "$"+_dccAddress.toString()+"$"; 211 makeRosterEntry(_rosterId); 212 _sessionProfile = null; 213 } 214 } 215 return true; 216 } 217 218 protected RosterEntry makeRosterEntry(String id) { 219 RosterEntry rosterEntry = new RosterEntry(); 220 rosterEntry.setId(id); 221 DccLocoAddress dccAddr = getDccAddress(); 222 if (dccAddr == null) { 223 return null; 224 } 225 rosterEntry.setDccAddress(String.valueOf(dccAddr.getNumber())); 226 rosterEntry.setProtocol(dccAddr.getProtocol()); 227 rosterEntry.ensureFilenameExists(); 228 return rosterEntry; 229 } 230 231 /** 232 * Sets dccAddress and key for a speedProfile. Will fetch RosterEntry if one exists. 233 * If _rosterEntry exists, _rosterId set to RosterEntry Id (which may or not be "id") 234 * else _rosterId set to "id" or decoder address. 235 * Called from: 236 * DefaultConditional.takeActionIfNeeded() - execute a setDccAddress action 237 * SpeedUtil.makeSpeedTree() - need to use track speeds 238 * WarrantFrame.checkTrainId() - about to run, assures address is set 239 * Warrantroute.getRoster() - selection form _rosterBox 240 * WarrantRoute.setAddress() - whatever is in _dccNumBox.getText() 241 * WarrantRoute.setTrainPanel() - whatever in _dccNumBox.getText() 242 * WarrantTableModel.setValue() - whatever address is put into the ADDRESS_COLUMN 243 * @param id address as a String, either RosterEntryTitle or decoder address 244 * @return true if address found for id 245 */ 246 public boolean setAddress(String id) { 247 log.trace("setDccAddress: id= {}, _rosterId= {}", id, _rosterId); 248 if (id == null || id.isEmpty()) { 249 return false; 250 } 251 if (setRosterId(id)) { 252 return true; 253 } 254 int index = - 1; 255 for (int i=0; i<id.length(); i++) { 256 if (!Character.isDigit(id.charAt(i))) { 257 index = i; 258 break; 259 } 260 } 261 String numId; 262 String type; 263 if (index == -1) { 264 numId = id; 265 type = null; 266 } else { 267 int beginIdx; 268 int endIdx; 269 if (id.charAt(index) == '(') { 270 beginIdx = index + 1; 271 } else { 272 beginIdx = index; 273 } 274 if (id.charAt(id.length() - 1) == ')') { 275 endIdx = id.length() - 1; 276 } else { 277 endIdx = id.length(); 278 } 279 numId = id.substring(0, index); 280 type = id.substring(beginIdx, endIdx); 281 } 282 283 int num; 284 try { 285 num = Integer.parseInt(numId); 286 } catch (NumberFormatException e) { 287 num = 0; 288 } 289 if (type == null) { 290 if (num > 128) { 291 type = "L"; 292 } else { 293 type = "S"; 294 } 295 } 296 if (!setDccAddress(num, type)) { 297 log.error("setDccAddress failed for ID= {} number={} type={}", id, num, type); 298 return false; 299 } else if (log.isTraceEnabled()) { 300 log.debug("setDccAddress({}): _rosterId= {}, _dccAddress= {}", 301 id, _rosterId, _dccAddress.toString()); 302 } 303 return true; 304 } 305 306 // Possibly customize these ramping values per warrant or loco later 307 // for now use global values set in WarrantPreferences 308 // user's ramp speed increase amount 309 protected float getRampThrottleIncrement() { 310 if (_rampThrottleIncrement <= 0) { 311 _rampThrottleIncrement = WarrantPreferences.getDefault().getThrottleIncrement(); 312 } 313 return _rampThrottleIncrement; 314 } 315 316 protected void setRampThrottleIncrement(float incr) { 317 _rampThrottleIncrement = incr; 318 } 319 320 protected int getRampTimeIncrement() { 321 if (_rampTimeIncrement < 500) { 322 _rampTimeIncrement = WarrantPreferences.getDefault().getTimeIncrement(); 323 if (_rampTimeIncrement <= 500) { 324 _rampTimeIncrement = 500; 325 } 326 } 327 return _rampTimeIncrement; 328 } 329 330 protected void setRampTimeIncrement(int incr) { 331 _rampTimeIncrement = incr; 332 } 333 334 /** ms momentum time to change speed for a throttle amount 335 * @param fromSpeed throttle change 336 * @param toSpeed throttle change 337 * @return momentum time 338 */ 339 protected float getMomentumTime(float fromSpeed, float toSpeed) { 340 float incr = getThrottleSpeedStepIncrement(); // step amount 341 float time; 342 float delta; 343 if (fromSpeed < toSpeed) { 344 delta = toSpeed - fromSpeed; 345 time = _ma * delta / incr; // accelerating 346 } else { 347 delta = fromSpeed - toSpeed; 348 time = _md * delta / incr; 349 } 350 // delta / incr ought to be number of speed steps 351 if (time < 2 * delta / incr) { 352 time = 2 * delta / incr; // Even with CV == 0, there must be some time to change speed 353 } 354 if (log.isTraceEnabled()) { 355 log.debug("getMomentumTime for {}, addr={}. fromSpeed={}, toSpeed= {}, time= {}ms for {} steps", 356 _rosterId, getAddress(), fromSpeed, toSpeed, time, delta / incr); 357 } 358 return time; 359 } 360 361 /** 362 * throttle's minimum speed change amount 363 * @return speed step amount 364 */ 365 protected float getThrottleSpeedStepIncrement() { 366 // JMRI throttles don't seem to get actual values 367 if (_throttle != null) { 368 return _throttle.getSpeedIncrement(); 369 } 370 return 1.0f / 126.0f; 371 } 372 373 // treeMap implementation in _mergeProfile is not synchronized 374 protected synchronized RosterSpeedProfile getMergeProfile() { 375 if (_sessionProfile == null) { 376 makeSpeedTree(); 377 makeRampParameters(); 378 } 379 return _sessionProfile; 380 } 381 382 private synchronized void makeSpeedTree() { 383 log.trace("makeSpeedTree for {}.", _rosterId); 384 WarrantManager manager = InstanceManager.getDefault(WarrantManager.class); 385 _sessionProfile = manager.getMergeProfile(_rosterId); 386 if (_sessionProfile == null) { 387 _rosterEntry = Roster.getDefault().getEntryForId(_rosterId); 388 RosterSpeedProfile profile; 389 if (_rosterEntry == null) { 390 _rosterEntry = makeRosterEntry(_rosterId); 391 profile = new RosterSpeedProfile(_rosterEntry); 392 } else { 393 profile = _rosterEntry.getSpeedProfile(); 394 if (profile == null) { 395 profile = new RosterSpeedProfile(_rosterEntry); 396 _rosterEntry.setSpeedProfile(profile); 397 } 398 } 399 _sessionProfile = manager.makeProfileCopy(profile, _rosterEntry); 400 manager.setMergeProfile(_rosterId, _sessionProfile); 401 } 402 403 if (log.isTraceEnabled()) { 404 log.debug("SignalSpeedMap: throttle factor= {}, layout scale= {} convesion to mm/s= {}", 405 _signalSpeedMap.getDefaultThrottleFactor(), _signalSpeedMap.getLayoutScale(), 406 _signalSpeedMap.getDefaultThrottleFactor() * _signalSpeedMap.getLayoutScale() / SCALE_FACTOR); 407 } 408 } 409 410 private void makeRampParameters() { 411 _rampTimeIncrement = getRampTimeIncrement(); // get a value if not already set 412 _rampThrottleIncrement = getRampThrottleIncrement(); 413 // default cv setting of momentum speed change per 1% of throttle increment 414 _ma = 0; // time needed to accelerate one throttle speed step 415 _md = 0; // time needed to decelerate one throttle speed step 416 if (_rosterEntry!=null) { 417 String fileName = Roster.getDefault().getRosterFilesLocation() + _rosterEntry.getFileName(); 418 Element elem; 419 XmlFile xmlFile = new XmlFile() {}; 420 try { 421 elem = xmlFile.rootFromFile(new File(fileName)); 422 } catch (FileNotFoundException npe) { 423 elem = null; 424 } catch (IOException | JDOMException eb) { 425 log.error("Exception while loading warrant preferences",eb); 426 elem = null; 427 } 428 if (elem != null) { 429 elem = elem.getChild("locomotive"); 430 } 431 if (elem != null) { 432 elem = elem.getChild("values"); 433 } 434 if (elem != null) { 435 List<Element> list = elem.getChildren("CVvalue"); 436 int count = 0; 437 for (Element cv : list) { 438 Attribute attr = cv.getAttribute("name"); 439 if (attr != null) { 440 if (attr.getValue().equals("3")) { 441 _ma += getMomentumFactor(cv); 442 count++; 443 } else if (attr.getValue().equals("4")) { 444 _md += getMomentumFactor(cv); 445 count++; 446 } else if (attr.getValue().equals("23")) { 447 _ma += getMomentumAdustment(cv); 448 count++; 449 } else if (attr.getValue().equals("24")) { 450 _md += getMomentumAdustment(cv); 451 count++; 452 } 453 } 454 if (count > 3) { 455 break; 456 } 457 } 458 } 459 } 460 if (log.isDebugEnabled()) { 461 log.debug("makeRampParameters for {}, addr={}. _ma= {}ms/step, _md= {}ms/step. rampThrottleIncr= {} rampTimeIncr= {} throttleStep= {}", 462 _rosterId, getAddress(), _ma, _md, _rampThrottleIncrement, _rampTimeIncrement, getThrottleSpeedStepIncrement()); 463 } 464 } 465 466 // return milliseconds per one speed step 467 private int getMomentumFactor(Element cv) { 468 Attribute attr = cv.getAttribute("value"); 469 int num = 0; 470 if (attr != null) { 471 try { 472 /* .896sec per (throttle Speed Step Increment) is NMRA spec for each CV value 473 CV#3 474 Determines the decoder's acceleration rate. The formula for the acceleration rate shall be equal to (the contents 475 of CV#3*.896)/(number of speed steps in use). For example, if the contents of CV#3 =2, then the acceleration 476 is 0.064 sec/step for a decoder currently using 28 speed steps. If the content of this parameter equals "0" then 477 there is no programmed momentum during acceleration. 478 Same for CV#24 479 */ 480 num = Integer.parseInt( attr.getValue()); 481 // reciprocal of getThrottleSpeedStepIncrement() is number of steps in use 482 num = Math.round(num * 896 * getThrottleSpeedStepIncrement()); // milliseconds per step 483 } catch (NumberFormatException nfe) { 484 num = 0; 485 } 486 } 487 if ( log.isTraceEnabled() ) { 488 log.trace("getMomentumFactor for cv {} {}, num= {}", 489 cv.getAttribute("name"), attr, num); 490 } 491 return num; 492 } 493 494 // return milliseconds per one speed step 495 private int getMomentumAdustment(Element cv) { 496 /* .896sec per is NMRA spec for each CV value 497 CV#23 498 This Configuration Variable contains additional acceleration rate information that is to be added to or 499 subtracted from the base value contained in Configuration Variable #3 using the formula (the contents of 500 CV#23*.896)/(number of speed steps in use). This is a 7 bit value (bits 0-6) with bit 7 being reserved for a 501 sign bit (0-add, 1-subtract). In case of overflow the maximum acceleration rate shall be used. In case of 502 160 underflow no acceleration shall be used. The expected use is for changing momentum to simulate differing 503 train lengths/loads, most often when operating in a consist. 504 Same for CV#24 505 */ 506 Attribute attr = cv.getAttribute("value"); 507 int num = 0; 508 if (attr != null) { 509 try { 510 int val = Integer.parseInt(attr.getValue()); 511 num = val & 0x3F; //value is 6 bits 512 if ((val & 0x40) != 0) { // 7th bit sign 513 num = -num; 514 } 515 } catch (NumberFormatException nfe) { 516 num = 0; 517 } 518 } 519 if ( log.isTraceEnabled()) { 520 log.trace("getMomentumAdustment for cv {} {}, num= {}", 521 cv.getAttribute("name"), attr, num); 522 } 523 return num; 524 } 525 526 protected boolean profileHasSpeedInfo() { 527 RosterSpeedProfile speedProfile = getMergeProfile(); 528 if (speedProfile == null) { 529 return false; 530 } 531 return (speedProfile.hasForwardSpeeds() || speedProfile.hasReverseSpeeds()); 532 } 533/* 534 private void mergeEntries(Entry<Integer, SpeedStep> sEntry, Entry<Integer, SpeedStep> mEntry) { 535 SpeedStep sStep = sEntry.getValue(); 536 SpeedStep mStep = mEntry.getValue(); 537 float sTrackSpeed = sStep.getForwardSpeed(); 538 float mTrackSpeed = mStep.getForwardSpeed(); 539 if (sTrackSpeed > 0) { 540 if (mTrackSpeed > 0) { 541 mTrackSpeed = (mTrackSpeed + sTrackSpeed) / 2; 542 } else { 543 mTrackSpeed = sTrackSpeed; 544 } 545 mStep.setForwardSpeed(mTrackSpeed); 546 } 547 sTrackSpeed = sStep.getReverseSpeed(); 548 mTrackSpeed = mStep.getReverseSpeed(); 549 if (sTrackSpeed > 0) { 550 if (sTrackSpeed > 0) { 551 if (mTrackSpeed > 0) { 552 mTrackSpeed = (mTrackSpeed + sTrackSpeed) / 2; 553 } else { 554 mTrackSpeed = sTrackSpeed; 555 } 556 } 557 mStep.setReverseSpeed(mTrackSpeed); 558 } 559 }*/ 560 561 protected void setIsForward(boolean direction) { 562 _isForward = direction; 563 if (_throttle != null) { 564 _throttle.setIsForward(direction); 565 } 566 } 567 568 protected boolean getIsForward() { 569 if (_throttle != null) { 570 _isForward = _throttle.getIsForward(); 571 } 572 return _isForward; 573 } 574 /************* runtime speed needs - throttle, engineer acquired ***************/ 575 576 /** 577 * @param throttle set DccThrottle 578 */ 579 protected void setThrottle( DccThrottle throttle) { 580 _throttle = throttle; 581 getMergeProfile(); 582 // adjust user's setting to be throttle speed step settings 583 float stepIncrement = _throttle.getSpeedIncrement(); 584 _rampThrottleIncrement = stepIncrement * Math.round(getRampThrottleIncrement()/stepIncrement); 585 if (log.isDebugEnabled()) { 586 log.debug("User's Ramp increment modified to {} ({} speed steps)", 587 _rampThrottleIncrement, Math.round(_rampThrottleIncrement/stepIncrement)); 588 } 589 } 590 591 protected DccThrottle getThrottle() { 592 return _throttle; 593 } 594 595 // return true if the speed named 'speed2' is strictly greater than that of 'speed1' 596 protected boolean secondGreaterThanFirst(String speed1, String speed2) { 597 if (speed2 == null) { 598 return false; 599 } 600 if (speed1 == null) { 601 return true; 602 } 603 if (speed1.equals(speed2)) { 604 return false; 605 } 606 float s1 = _signalSpeedMap.getSpeed(speed1); 607 float s2 = _signalSpeedMap.getSpeed(speed2); 608 return (s1 < s2); 609 } 610 611 /** 612 * Modify a throttle setting to match a speed name type 613 * Modification is done according to the interpretation of the speed name 614 * @param tSpeed throttle setting (current) 615 * @param sType speed type name 616 * @return modified throttle setting 617 */ 618 protected float modifySpeed(float tSpeed, String sType) { 619 log.trace("modifySpeed speed= {} for SpeedType= \"{}\"", tSpeed, sType); 620 if (sType.equals(Warrant.Stop)) { 621 return 0.0f; 622 } 623 if (sType.equals(Warrant.EStop)) { 624 return -1.0f; 625 } 626 float throttleSpeed = tSpeed; // throttleSpeed is a throttle setting 627 if (sType.equals(Warrant.Normal)) { 628 return throttleSpeed; 629 } 630 float signalSpeed = _signalSpeedMap.getSpeed(sType); 631 632 switch (_signalSpeedMap.getInterpretation()) { 633 case SignalSpeedMap.PERCENT_NORMAL: 634 throttleSpeed *= signalSpeed / 100; // ratio of normal 635 break; 636 case SignalSpeedMap.PERCENT_THROTTLE: 637 signalSpeed /= 100; // ratio of full throttle setting 638 if (signalSpeed < throttleSpeed) { 639 throttleSpeed = signalSpeed; 640 } 641 break; 642 643 case SignalSpeedMap.SPEED_MPH: // convert miles per hour to track speed 644 signalSpeed /= _signalSpeedMap.getLayoutScale(); 645 signalSpeed /= 2.2369363f; // layout track speed mph -> mm/ms 646 float trackSpeed = getTrackSpeed(throttleSpeed); 647 if (signalSpeed < trackSpeed) { 648 throttleSpeed = getThrottleSettingForSpeed(signalSpeed); 649 } 650 break; 651 652 case SignalSpeedMap.SPEED_KMPH: 653 signalSpeed /= _signalSpeedMap.getLayoutScale(); 654 signalSpeed /= 3.6f; // layout track speed mm/ms -> km/hr 655 trackSpeed = getTrackSpeed(throttleSpeed); 656 if (signalSpeed < trackSpeed) { 657 throttleSpeed = getThrottleSettingForSpeed(signalSpeed); 658 } 659 break; 660 default: 661 log.error("Unknown speed interpretation {}", _signalSpeedMap.getInterpretation()); 662 throw new java.lang.IllegalArgumentException( 663 "Unknown speed interpretation " + _signalSpeedMap.getInterpretation()); 664 } 665 if (log.isTraceEnabled()) { 666 log.trace("modifySpeed: from {}, to {}, signalSpeed= {}. interpretation= {}", 667 tSpeed, throttleSpeed, signalSpeed, _signalSpeedMap.getInterpretation()); 668 } 669 return throttleSpeed; 670 } 671 672 /** 673 * A a train's speed at a given throttle setting and time would travel a distance. 674 * return the time it would take for the train at another throttle setting to 675 * travel the same distance. 676 * @param speed a given throttle setting 677 * @param time a given time 678 * @param modifiedSpeed a different speed setting 679 * @return the time to travel the same distance at the different setting 680 */ 681 protected static long modifyTime(float speed, long time, float modifiedSpeed) { 682 if (Math.abs(speed - modifiedSpeed) > .0001f) { 683 return (long)((speed / modifiedSpeed) * time); 684 } else { 685 return time; 686 } 687 } 688 689 /** 690 * Get the track speed in millimeters per millisecond (= meters/sec) 691 * If SpeedProfile has no speed information an estimate is given using the WarrantPreferences 692 * throttleFactor. 693 * NOTE: Call profileHasSpeedInfo() first to determine if a reliable speed is known. 694 * for a given throttle setting and direction. 695 * SpeedProfile returns 0 if it has no speed information 696 * @param throttleSetting throttle setting 697 * @return track speed in millimeters/millisecond (not mm/sec) 698 */ 699 protected float getTrackSpeed(float throttleSetting) { 700 if (throttleSetting <= 0.0f) { 701 return 0.0f; 702 } 703 if (_dccAddress == null) { 704 return factorSpeed(throttleSetting); 705 } 706 RosterSpeedProfile sessionProfile = getMergeProfile(); 707 boolean isForward = getIsForward(); 708 // Note SpeedProfile uses millimeters per second. 709 float speed = sessionProfile.getSpeed(throttleSetting, isForward) / 1000; 710 if (speed <= 0.0f) { 711 speed = sessionProfile.getSpeed(throttleSetting, !isForward) / 1000; 712 } 713 if (speed <= 0.0f) { 714 return factorSpeed(throttleSetting); 715 } 716 return speed; 717 } 718 719 720 private float factorSpeed(float throttleSetting) { 721 float factor = _signalSpeedMap.getDefaultThrottleFactor() * SCALE_FACTOR / _signalSpeedMap.getLayoutScale(); 722 return throttleSetting * factor; 723 } 724 /** 725 * Get the throttle setting needed to achieve a given track speed 726 * track speed is mm/ms. SpeedProfile wants mm/s 727 * SpeedProfile returns 0 if it has no speed information 728 * @param trackSpeed in millimeters per millisecond (m/s) 729 * @return throttle setting or 0 730 */ 731 protected float getThrottleSettingForSpeed(float trackSpeed) { 732 RosterSpeedProfile speedProfile = getMergeProfile(); 733 float throttleSpeed; 734 if (speedProfile != null) { 735 throttleSpeed = speedProfile.getThrottleSetting(trackSpeed * 1000, getIsForward()); 736 } else { 737 throttleSpeed = 0f; 738 } 739 if (throttleSpeed <= 0.0f) { 740 throttleSpeed = trackSpeed * _signalSpeedMap.getLayoutScale() / 741 (SCALE_FACTOR *_signalSpeedMap.getDefaultThrottleFactor()); 742 } 743 return throttleSpeed; 744 } 745 746 /** 747 * Get distance traveled at a constant speed. If this is called at 748 * a speed change the throttleSetting should be modified to reflect the 749 * average speed over the time interval. 750 * @param speedSetting Recorded (Normal) throttle setting 751 * @param speedtype speed name to modify throttle setting to get modified speed 752 * @param time milliseconds 753 * @return distance in millimeters 754 */ 755 protected float getDistanceTraveled(float speedSetting, String speedtype, float time) { 756 if (time <= 0) { 757 return 0; 758 } 759 float throttleSetting = modifySpeed(speedSetting, speedtype); 760 return getTrackSpeed(throttleSetting) * time; 761 } 762 763 /** 764 * Get time needed to travel a distance at a constant speed. 765 * @param throttleSetting Throttle setting 766 * @param distance in millimeters 767 * @return time in milliseconds 768 */ 769 protected int getTimeForDistance(float throttleSetting, float distance) { 770 float speed = getTrackSpeed(throttleSetting); 771 if (distance <= 0 || speed <= 0) { 772 return 0; 773 } 774 return Math.round(distance/speed); 775 } 776 777 /*************** Block Speed Info *****************/ 778 /** 779 * build map of BlockSpeedInfo's for the route. Map corresponds to list 780 * of BlockOrders of a Warrant 781 * @param commands list of script commands 782 * @param orders list of BlockOrders 783 */ 784 protected void getBlockSpeedTimes(List<ThrottleSetting> commands, List<BlockOrder> orders) { 785 _speedInfo = new ArrayList<>(); 786 float firstSpeed = 0.0f; // used for entrance 787 float speed = 0.0f; 788 float intStartSpeed = 0.0f; 789 float intEndSpeed = 0.0f; 790 long blkTime = 0; 791 float pathDist = 0; 792 float calcDist = 0; 793 int firstIdx = 0; // for all blocks except first, this is index of NOOP command 794 int blkOrderIdx = 0; 795 ThrottleSetting ts = commands.get(0); 796 OBlock blk = (OBlock)ts.getNamedBeanHandle().getBean(); 797 String blkName = blk.getDisplayName(); 798 for (int i = 0; i < commands.size(); i++) { 799 ts = commands.get(i); 800 Command command = ts.getCommand(); 801 CommandValue cmdVal = ts.getValue(); 802 if (command.equals(Command.FORWARD)) { 803 ValueType val = cmdVal.getType(); 804 setIsForward(val.equals(ValueType.VAL_TRUE)); 805 } 806 long time = ts.getTime(); 807 blkTime += time; 808 if (time > 0) { 809 calcDist += getDistanceOfSpeedChange(intStartSpeed, intEndSpeed, time); 810 } 811 if (command.equals(Command.SPEED)) { 812 speed = cmdVal.getFloat(); 813 if (speed < 0) { 814 speed = 0; 815 } 816 intStartSpeed = intEndSpeed; 817 intEndSpeed = speed; 818 } 819 if (command.equals(Command.NOOP)) { 820 // make map entry. First measure distance to end of block 821 if (time > 0) { 822 calcDist += getDistanceOfSpeedChange(intStartSpeed, intEndSpeed, time); 823 } 824 float ratio = 1; 825 if (calcDist > 0 && blkOrderIdx > 0 && blkOrderIdx < commands.size() - 1) { 826 pathDist = orders.get(blkOrderIdx).getPathLength(); 827 ratio = pathDist / calcDist; 828 } else { 829 pathDist = orders.get(blkOrderIdx).getPathLength() / 2; 830 } 831 _speedInfo.add(new BlockSpeedInfo(blkName, firstSpeed, speed, blkTime, pathDist, calcDist, firstIdx, i)); 832 if (Warrant._trace || log.isDebugEnabled()) { 833 if (calcDist <= 0 || Math.abs(ratio) > 2.0f || Math.abs(ratio) < 0.5f) { 834 log.debug("\"{}\" Speeds: enter= {}, exit= {}. time= {}ms, pathDist= {}, calcDist= {}. index {} to {}", 835 blkName, firstSpeed, speed, blkTime, pathDist, calcDist, firstIdx, i); 836 } 837 } 838 blkOrderIdx++; 839 blk = (OBlock)ts.getNamedBeanHandle().getBean(); 840 blkName = blk.getDisplayName(); 841 blkTime = 0; 842 calcDist = 0; 843 intStartSpeed = intEndSpeed; 844 firstSpeed = speed; 845 firstIdx = i + 1; // first in next block is next index 846 } 847 // set up recording track speeds 848 } 849 _speedInfo.add(new BlockSpeedInfo(blkName, firstSpeed, speed, blkTime, pathDist, calcDist, firstIdx, commands.size() - 1)); 850 if (log.isDebugEnabled()) { 851 log.debug("block: {} speeds: entrance= {}, exit= {}. time= {}ms pathDist= {}, calcDist= {}. index {} to {}", 852 blkName, firstSpeed, speed, blkTime, pathDist, calcDist, firstIdx, (commands.size() - 1)); 853 } 854 clearStats(-1); 855 _intStartSpeed = 0; 856 _intEndSpeed = 0; 857 } 858 859 protected BlockSpeedInfo getBlockSpeedInfo(int idxBlockOrder) { 860 return _speedInfo.get(idxBlockOrder); 861 } 862 863 /** 864 * Get the ramp for a speed change from Throttle settings 865 * @param fromSpeed - starting speed setting 866 * @param toSpeed - ending speed setting 867 * @return ramp data 868 */ 869 protected RampData getRampForSpeedChange(float fromSpeed, float toSpeed) { 870 return new RampData(this, getRampThrottleIncrement(), getRampTimeIncrement(), fromSpeed, toSpeed); 871 } 872 873 /** 874 * Get the ramp length for a speed change from Throttle settings 875 * @param fromSpeed - starting speed setting 876 * @param toSpeed - ending speed setting 877 * @return ramp length 878 */ 879 protected float getRampLengthForEntry(float fromSpeed, float toSpeed) { 880 RampData ramp = getRampForSpeedChange(fromSpeed, toSpeed); 881 float enterLen = ramp.getRampLength(); 882 if (log.isTraceEnabled()) { 883 log.debug("getRampLengthForEntry: from speed={} to speed={}. rampLen={}", 884 fromSpeed, toSpeed, enterLen); 885 } 886 return enterLen; 887 } 888 889 /** 890 * Return the distance traveled at current speed after a speed change was made. 891 * Takes into account the momentum configured for the decoder to change from 892 * the previous speed to the current speed. Assumes the velocity change is linear. 893 * Does not return a distance greater than that needed by momentum time. 894 * 895 * @param fromSpeed throttle setting when speed changed to toSpeed 896 * @param toSpeed throttle setting being set 897 * @param speedTime elapsed time from when the speed change was made to now 898 * @return distance traveled 899 */ 900 protected float getDistanceOfSpeedChange(float fromSpeed, float toSpeed, long speedTime) { 901 if (toSpeed < 0) { 902 toSpeed = 0; 903 } 904 if (fromSpeed < 0) { 905 fromSpeed = 0; 906 } 907 float momentumTime = getMomentumTime(fromSpeed, toSpeed); 908 float dist; 909 // assume a linear change of speed 910 if (speedTime <= momentumTime ) { 911 // perhaps will be too far since toSpeed may not be attained 912 dist = getTrackSpeed((fromSpeed + toSpeed)/2) * speedTime; 913 } else { 914 dist = getTrackSpeed((fromSpeed + toSpeed)/2) * momentumTime; 915 if (speedTime > momentumTime) { // time remainder at changed speed 916 dist += getTrackSpeed(toSpeed) * (speedTime - momentumTime); 917 } 918 } 919// log.debug("momentumTime = {}, speedTime= {} moDist= {}", momentumTime, speedTime, dist); 920 return dist; 921 } 922 923 /*************** dynamic calibration ***********************/ 924 private long _timeAtSpeed = 0; 925 private float _intStartSpeed = 0.0f; 926 private float _intEndSpeed = 0.0f; 927 private float _distanceTravelled = 0; 928 private float _settingsTravelled = 0; 929 private long _prevChangeTime = -1; 930 private int _numchanges = 0; // number of time changes within the block 931 private long _entertime = 0; // entrance time to block 932 private boolean _cantMeasure = false; // speed has at 0 at some time while in the block 933 934 /** 935 * Just entered a new block at 'toTime'. Do the calculation of speed of the 936 * previous block from when the previous block block was entered. 937 * 938 * Throttle changes within the block will cause different speeds. We attempt 939 * to accumulate these time and distances to calculate a weighted speed average. 940 * See method speedChange() below. 941 * @param blkIdx BlockOrder index of the block the engine just left. (not train) 942 * The lead engine just entered the next block after blkIdx. 943 */ 944 protected void leavingBlock(int blkIdx) { 945 long exitTime = System.currentTimeMillis(); 946 BlockSpeedInfo blkInfo = getBlockSpeedInfo(blkIdx); 947 log.debug("BlockInfo: {}", blkInfo); 948 949 if (_cantMeasure) { 950 clearStats(exitTime); 951 _entertime = exitTime; // entry of next block 952 log.debug("Skip speed measurement"); 953 return; 954 } 955 boolean isForward = getIsForward(); 956 float throttle = _throttle.getSpeedSetting(); // may not be a multiple of a speed step 957 float length = blkInfo.getPathLen(); 958 long elapsedTime = exitTime - _prevChangeTime; 959 if (_numchanges == 0) { 960 _distanceTravelled = getTrackSpeed(throttle) * elapsedTime; 961 _settingsTravelled = throttle * elapsedTime; 962 _timeAtSpeed = elapsedTime; 963 } else { 964 float dist = getDistanceOfSpeedChange(_intStartSpeed, _intEndSpeed, elapsedTime); 965 if (_intStartSpeed > 0 || _intEndSpeed > 0) { 966 _timeAtSpeed += elapsedTime; 967 } 968 if (log.isDebugEnabled()) { 969 log.debug("speedChange to {}: dist={} in {}ms from speed {} to {}.", 970 throttle, dist, elapsedTime, _intStartSpeed, _intEndSpeed); 971 } 972 _distanceTravelled += dist; 973 _settingsTravelled += throttle * elapsedTime; 974 } 975 976 float measuredSpeed; 977 float distRatio; 978 if (length <= 0) { 979 // Origin and Destination block lengths immaterial 980 measuredSpeed = _distanceTravelled / _timeAtSpeed; 981 distRatio = 2; // actual start and end positions unknown 982 } else { 983 measuredSpeed = length / _timeAtSpeed; 984 distRatio = blkInfo.getCalcLen()/_distanceTravelled; 985 } 986 measuredSpeed *= 1000; // SpeedProfile is mm/sec 987 float aveSettings = _settingsTravelled / _timeAtSpeed; 988 if (log.isDebugEnabled()) { 989 float timeRatio = (exitTime - _entertime) / (float)_timeAtSpeed; 990 log.debug("distRatio= {}, timeRatio= {}, aveSpeed= {}, length= {}, calcLength= {}, elapsedTime= {}", 991 distRatio, timeRatio, measuredSpeed, length, _distanceTravelled, (exitTime - _entertime)); 992 } 993 if (aveSettings > 1.0 || measuredSpeed > MAX_TGV_SPEED*aveSettings/_signalSpeedMap.getLayoutScale() 994 || distRatio > 1.15f || distRatio < 0.87f) { 995 if (log.isDebugEnabled()) { 996 // We assume bullet train's speed is linear from 0 throttle to max throttle. 997 // we also tolerate distance calculation errors up to 20% longer or shorter 998 log.info("Bad speed measurements data for block {}. aveThrottle= {}, " + 999 " measuredSpeed= {},(TGVmax= {}), distTravelled= {}, pathLen= {}", 1000 blkInfo.getBlockDisplayName(), aveSettings, measuredSpeed, 1001 MAX_TGV_SPEED*aveSettings/_signalSpeedMap.getLayoutScale(), 1002 _distanceTravelled, length); 1003 } 1004 } else if (_numchanges < 3) { 1005 setSpeedProfile(_sessionProfile, aveSettings, measuredSpeed, isForward); 1006 } 1007 if (log.isDebugEnabled()) { 1008 log.debug("{} changes in block \'{}\". measuredDist={}, pathLen={}, " + 1009 " measuredThrottle={}, measuredTrkSpd={}, profileTrkSpd={} curThrottle={}.", 1010 _numchanges, blkInfo.getBlockDisplayName(), Math.round(_distanceTravelled), length, 1011 aveSettings, measuredSpeed, getTrackSpeed(aveSettings)*1000, throttle); 1012 } 1013 clearStats(exitTime); 1014 _entertime = exitTime; // entry of next block 1015 } 1016 1017 // average with existing entry, if possible 1018 private void setSpeedProfile(RosterSpeedProfile profile, float throttle, float measuredSpeed, boolean isForward) { 1019 int keyIncrement = Math.round(getThrottleSpeedStepIncrement() * 1000); 1020 TreeMap<Integer, SpeedStep> speeds = profile.getProfileSpeeds(); 1021 int key = Math.round(throttle * 1000); 1022 Entry<Integer, SpeedStep> entry = speeds.floorEntry(key); 1023 if (entry != null && mergeEntry(key, measuredSpeed, entry, keyIncrement, isForward)) { 1024 return; 1025 } 1026 entry = speeds.ceilingEntry(key); 1027 if (entry != null && mergeEntry(key, measuredSpeed, entry, keyIncrement, isForward)) { 1028 return; 1029 } 1030 1031 float speed = profile.getSpeed(throttle, isForward); 1032 if (speed > 0.0f) { 1033 measuredSpeed = (measuredSpeed + speed) / 2; 1034 } 1035 1036 if (isForward) { 1037 profile.setForwardSpeed(throttle, measuredSpeed, _throttle.getSpeedIncrement()); 1038 } else { 1039 profile.setReverseSpeed(throttle, measuredSpeed, _throttle.getSpeedIncrement()); 1040 } 1041 log.debug("Put measuredThrottle={} and measuredTrkSpd={} for isForward= {} curThrottle={}.", 1042 throttle, measuredSpeed, isForward, throttle); 1043 } 1044 1045 private boolean mergeEntry(int key, float measuredSpeed, Entry<Integer, SpeedStep> entry, int keyIncrement, boolean isForward) { 1046 Integer sKey = entry.getKey(); 1047 if (Math.abs(sKey - key) < keyIncrement) { 1048 SpeedStep sStep = entry.getValue(); 1049 float sTrackSpeed; 1050 if (isForward) { 1051 sTrackSpeed = sStep.getForwardSpeed(); 1052 if (sTrackSpeed > 0) { 1053 if (sTrackSpeed > 0) { 1054 sTrackSpeed = (sTrackSpeed + measuredSpeed) / 2; 1055 } else { 1056 sTrackSpeed = measuredSpeed; 1057 } 1058 sStep.setForwardSpeed(sTrackSpeed); 1059 } 1060 } else { 1061 sTrackSpeed = sStep.getReverseSpeed(); 1062 if (sTrackSpeed > 0) { 1063 if (sTrackSpeed > 0) { 1064 sTrackSpeed = (sTrackSpeed + measuredSpeed) / 2; 1065 } else { 1066 sTrackSpeed = measuredSpeed; 1067 } 1068 sStep.setReverseSpeed(sTrackSpeed); 1069 } 1070 } 1071 } 1072 return false; 1073 } 1074 1075 private void clearStats(long exitTime) { 1076 _timeAtSpeed = 0; 1077 _distanceTravelled = 0.0f; 1078 _settingsTravelled = 0.0f; 1079 _numchanges = 0; 1080 _prevChangeTime = exitTime; 1081 _cantMeasure = false; 1082 } 1083 1084 /** 1085 * The engineer makes this notification before setting a new speed. 1086 * Calculate the distance traveled since the last speed change. 1087 * @param throttleSetting the new Speed of the Throttle. 1088 */ 1089 protected synchronized void speedChange(float throttleSetting) { 1090 if (Math.abs(_intEndSpeed - throttleSetting) < 0.00001f) { 1091 _cantMeasure = true; 1092 return; 1093 } 1094 _numchanges++; 1095 long time = System.currentTimeMillis(); 1096 if (throttleSetting <= 0) { 1097 throttleSetting = 0; 1098 } 1099 if (_prevChangeTime > 0) { 1100 long elapsedTime = time - _prevChangeTime; 1101 float dist = getDistanceOfSpeedChange(_intStartSpeed, _intEndSpeed, elapsedTime); 1102 if (dist > 0) { 1103 _timeAtSpeed += elapsedTime; 1104 } 1105 if (log.isTraceEnabled()) { 1106 log.debug("speedChange to {}: dist={} in {}ms from speed {} to {}.", 1107 throttleSetting, dist, elapsedTime, _intStartSpeed, _intEndSpeed); 1108 } 1109 _distanceTravelled += dist; 1110 _settingsTravelled += throttleSetting * elapsedTime; 1111 } 1112 if (_entertime <= 0) { 1113 _entertime = time; // time of first non-zero speed 1114 } 1115 _prevChangeTime = time; 1116 _intStartSpeed = _intEndSpeed; 1117 _intEndSpeed = throttleSetting; 1118 } 1119 1120 private static final org.slf4j.Logger log = org.slf4j.LoggerFactory.getLogger(SpeedUtil.class); 1121 1122}