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