001/* 002 * Copyright (C) Cross The Road Electronics. All rights reserved. 003 * License information can be found in CTRE_LICENSE.txt 004 * For support and suggestions contact support@ctr-electronics.com or file 005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases 006 */ 007package com.ctre.phoenix6; 008 009import static edu.wpi.first.units.Units.*; 010 011import com.ctre.phoenix6.Timestamp.TimestampSource; 012import com.ctre.phoenix6.hardware.DeviceIdentifier; 013import com.ctre.phoenix6.jni.ErrorReportingJNI; 014import com.ctre.phoenix6.jni.StatusSignalJNI; 015 016import edu.wpi.first.units.Measure; 017import edu.wpi.first.units.PerUnit; 018import edu.wpi.first.units.TimeUnit; 019import edu.wpi.first.units.Unit; 020import edu.wpi.first.units.measure.*; 021 022/** 023 * Class that provides operations to retrieve 024 * information about a status signal. 025 */ 026public abstract class BaseStatusSignal { 027 protected DeviceIdentifier deviceIdentifier; 028 protected int spn; 029 protected String units; 030 protected StatusCode error = StatusCode.StatusCodeNotInitialized; 031 protected double baseValue = 0; 032 protected AllTimestamps timestamps = new AllTimestamps(); 033 protected final String signalName; 034 035 protected Runnable _reportIfOldFunc; 036 037 private double _lastTimestamp = 0.0; 038 039 protected StatusSignalJNI jni = new StatusSignalJNI(); 040 041 BaseStatusSignal(DeviceIdentifier deviceIdentifier, int spn, String signalName, Runnable reportIfOldFunc) { 042 this.deviceIdentifier = deviceIdentifier; 043 this.spn = spn; 044 this.signalName = signalName; 045 046 this._reportIfOldFunc = reportIfOldFunc; 047 048 jni.network = deviceIdentifier.getNetwork(); 049 jni.deviceHash = deviceIdentifier.getDeviceHash(); 050 jni.spn = spn; 051 052 this.units = jni.JNI_GetUnits(); 053 } 054 055 /* Constructor for an invalid BaseStatusSignal */ 056 BaseStatusSignal(StatusCode error) { 057 this(new DeviceIdentifier(), 0, "Invalid", () -> {}); 058 this.error = error; 059 } 060 061 protected abstract void updateUnits(int spn); 062 063 /** 064 * Implementation of the waitForAll API. 065 * 066 * @param location Location of the calling function 067 * @param timeoutSeconds Maximum time to wait for all the signals 068 * @param signals Signals to wait on 069 * @return Status of the wait 070 */ 071 private static StatusCode waitForAllImpl(String location, double timeoutSeconds, BaseStatusSignal... signals) { 072 if (signals.length < 1) { 073 /* We don't have any signals to wait for, so return early */ 074 ErrorReportingJNI.reportStatusCode(StatusCode.InvalidParamValue.value, location); 075 return StatusCode.InvalidParamValue; 076 } 077 078 String network = signals[0].deviceIdentifier.getNetwork(); 079 StatusSignalJNI[] toGet = new StatusSignalJNI[signals.length]; 080 for (int i = 0; i < signals.length; ++i) { 081 var sig = signals[i]; 082 if (i != 0 && !sig.deviceIdentifier.getNetwork().equals(network)) { 083 // Networks don't match, return early 084 ErrorReportingJNI.reportStatusCode(StatusCode.InvalidNetwork.value, location); 085 return StatusCode.InvalidNetwork; 086 } 087 toGet[i] = sig.jni; 088 } 089 090 /* Report if any device firmware versions are too old */ 091 for (var signal : signals) { 092 signal._reportIfOldFunc.run(); 093 } 094 095 /* Now wait for all the signals */ 096 int err = StatusSignalJNI.JNI_WaitForAll(network, timeoutSeconds, toGet); 097 for (int i = 0; i < signals.length; ++i) { 098 signals[i].error = StatusCode.valueOf(toGet[i].statusCode); 099 signals[i].baseValue = toGet[i].value; 100 signals[i].timestamps.update( 101 toGet[i].swtimeStampSeconds, TimestampSource.System, true, 102 toGet[i].hwtimeStampSeconds, TimestampSource.CANivore, true, 103 toGet[i].ecutimeStampSeconds, TimestampSource.Device, toGet[i].ecutimeStampSeconds != 0.0 104 ); 105 signals[i].updateUnits(toGet[i].unitsKey); 106 } 107 108 /* error reporting */ 109 StatusCode retval = StatusCode.valueOf(err); 110 if (false == retval.isOK()) { 111 ErrorReportingJNI.reportStatusCode(retval.value, location); 112 } 113 return StatusCode.valueOf(err); 114 } 115 116 /** 117 * Waits for new data on all provided signals up to timeout. 118 * This API is typically used with CANivore Bus signals as they will be synced using the 119 * CANivore Timesync feature and arrive simultaneously. Signals on a roboRIO bus cannot 120 * be synced and may require a significantly longer blocking call to receive all signals. 121 * <p> 122 * Note that CANivore Timesync requires Phoenix Pro. 123 * <p> 124 * This can also be used with a timeout of zero to refresh many signals at once, which 125 * is faster than calling refresh() on every signal. This is equivalent to calling {@link #refreshAll}. 126 * <p> 127 * If a signal arrives multiple times while waiting, such as when *not* using CANivore 128 * Timesync, the newest signal data is fetched. Additionally, if this function times out, 129 * the newest signal data is fetched for all signals (when possible). We recommend checking 130 * the individual status codes using {@link #getStatus()} when this happens. 131 * 132 * @param timeoutSeconds Maximum time to wait for new data in seconds. 133 * Pass zero to refresh all signals without blocking. 134 * @param signals Signals to wait for new data against 135 * @return An InvalidParamValue if signals array is empty, 136 * InvalidNetwork if signals are on different CAN bus networks, 137 * RxTimeout if it took longer than timeoutSeconds to receive all the signals, 138 * MultiSignalNotSupported if using the roboRIO bus with more than one signal and a non-zero timeout. 139 * An OK status code means that all signals arrived within timeoutSeconds and they are all OK. 140 * 141 * Any other value represents the StatusCode of the first failed signal. 142 * Call getStatus() on each signal to determine which ones failed. 143 */ 144 public static StatusCode waitForAll(double timeoutSeconds, BaseStatusSignal... signals) { 145 return waitForAllImpl("ctre.phoenix6.BaseStatusSignal.waitForAll", timeoutSeconds, signals); 146 } 147 148 /** 149 * Performs a non-blocking refresh on all provided signals. 150 * <p> 151 * This provides a performance improvement over separately calling refresh() on each signal. 152 * 153 * @param signals Signals to refresh 154 * @return An InvalidParamValue if signals array is empty, 155 * InvalidNetwork if signals are on different CAN bus networks. 156 * An OK status code means that all signals are OK. 157 * 158 * Any other value represents the StatusCode of the first failed signal. 159 * Call getStatus() on each signal to determine which ones failed. 160 */ 161 public static StatusCode refreshAll(BaseStatusSignal... signals) { 162 return waitForAllImpl("ctre.phoenix6.BaseStatusSignal.refreshAll", 0, signals); 163 } 164 165 /** 166 * Performs latency compensation on signal using the signalSlope and signal's latency to determine 167 * the magnitude of compensation. The caller must refresh these StatusSignals beforehand; 168 * this function only does the math required for latency compensation. 169 * <p> 170 * This compensates for up to 300ms of latency by default. 171 * <p> 172 * <b>Example</b>: 173 * var compensatedTurns = BaseStatusSignal.getLatencyCompensatedValue(fx.getPosition(), fx.getVelocity()); 174 * 175 * @param signal Signal to be latency compensated. Caller must make sure this signal is up to date 176 * either by calling {@link StatusSignal#refresh()} or {@link StatusSignal#waitForUpdate(double)}. 177 * @param signalSlope Derivative of signal that informs compensation magnitude. Caller must make sure this 178 * signal is up to date either by calling {@link StatusSignal#refresh()} or 179 * {@link StatusSignal#waitForUpdate(double)}. 180 * @return Latency compensated value from the signal StatusSignal. 181 */ 182 public static 183 <U extends Unit, U_PER_SEC extends PerUnit<U, TimeUnit>, MEAS extends Measure<U>, MEAS_PER_SEC extends Measure<U_PER_SEC>> 184 Measure<U> getLatencyCompensatedValue(StatusSignal<MEAS> signal, StatusSignal<MEAS_PER_SEC> signalSlope) 185 { 186 return getLatencyCompensatedValue(signal, signalSlope, 0.300); 187 } 188 189 /** 190 * Performs latency compensation on signal using the signalSlope and signal's latency to determine 191 * the magnitude of compensation. The caller must refresh these StatusSignals beforehand; 192 * this function only does the math required for latency compensation. 193 * <p> 194 * <b>Example</b>: 195 * var compensatedTurns = BaseStatusSignal.getLatencyCompensatedValue(fx.getPosition(), fx.getVelocity()); 196 * 197 * @param signal Signal to be latency compensated. Caller must make sure this signal is up to date 198 * either by calling {@link StatusSignal#refresh()} or {@link StatusSignal#waitForUpdate(double)}. 199 * @param signalSlope Derivative of signal that informs compensation magnitude. Caller must make sure this 200 * signal is up to date either by calling {@link StatusSignal#refresh()} or 201 * {@link StatusSignal#waitForUpdate(double)}. 202 * @param maxLatencySeconds The maximum amount of latency to compensate for in seconds. A negative or zero 203 * value disables the max latency cap. This is used to cap the contribution of 204 * latency compensation for stale signals, such as after the device has been 205 * disconnected from the CAN bus. 206 * @return Latency compensated value from the signal StatusSignal. 207 */ 208 public static 209 <U extends Unit, U_PER_SEC extends PerUnit<U, TimeUnit>, MEAS extends Measure<U>, MEAS_PER_SEC extends Measure<U_PER_SEC>> 210 Measure<U> getLatencyCompensatedValue(StatusSignal<MEAS> signal, StatusSignal<MEAS_PER_SEC> signalSlope, double maxLatencySeconds) 211 { 212 final var nonCompensatedSignal = signal.getValue(); 213 final var changeInSignal = signalSlope.getValue(); 214 double latency = signal.getTimestamp().getLatency(); 215 if (maxLatencySeconds > 0.0 && latency > maxLatencySeconds) { 216 latency = maxLatencySeconds; 217 } 218 @SuppressWarnings("unchecked") 219 var retval = (MEAS)nonCompensatedSignal.plus((MEAS)changeInSignal.times(Seconds.of(latency))); 220 return retval; 221 } 222 223 /** 224 * Performs latency compensation on signal using the signalSlope and signal's latency to determine 225 * the magnitude of compensation. The caller must refresh these StatusSignals beforehand; 226 * this function only does the math required for latency compensation. 227 * <p> 228 * <b>Important</b>: The signalSlope must be the rate of change of the signal. If it is not the latency 229 * compensation may not perform as expected. 230 * <p> 231 * This compensates for up to 300ms of latency by default. 232 * <p> 233 * <b>Example</b>: 234 * double compensatedTurns = BaseStatusSignal.getLatencyCompensatedValueAsDouble(fx.getPosition(), fx.getVelocity()); 235 * 236 * @param signal Signal to be latency compensated. Caller must make sure this signal is up to date 237 * either by calling {@link StatusSignal#refresh()} or {@link StatusSignal#waitForUpdate(double)}. 238 * @param signalSlope Derivative of signal that informs compensation magnitude. Caller must make sure this 239 * signal is up to date either by calling {@link StatusSignal#refresh()} or 240 * {@link StatusSignal#waitForUpdate(double)}. 241 * @return Latency compensated value from the signal StatusSignal. 242 */ 243 public static double getLatencyCompensatedValueAsDouble(BaseStatusSignal signal, BaseStatusSignal signalSlope) 244 { 245 return getLatencyCompensatedValueAsDouble(signal, signalSlope, 0.300); 246 } 247 248 /** 249 * Performs latency compensation on signal using the signalSlope and signal's latency to determine 250 * the magnitude of compensation. The caller must refresh these StatusSignals beforehand; 251 * this function only does the math required for latency compensation. 252 * <p> 253 * <b>Important</b>: The signalSlope must be the rate of change of the signal. If it is not the latency 254 * compensation may not perform as expected. 255 * <p> 256 * <b>Example</b>: 257 * double compensatedTurns = BaseStatusSignal.getLatencyCompensatedValueAsDouble(fx.getPosition(), fx.getVelocity()); 258 * 259 * @param signal Signal to be latency compensated. Caller must make sure this signal is up to date 260 * either by calling {@link StatusSignal#refresh()} or {@link StatusSignal#waitForUpdate(double)}. 261 * @param signalSlope Derivative of signal that informs compensation magnitude. Caller must make sure this 262 * signal is up to date either by calling {@link StatusSignal#refresh()} or 263 * {@link StatusSignal#waitForUpdate(double)}. 264 * @param maxLatencySeconds The maximum amount of latency to compensate for in seconds. A negative or zero 265 * value disables the max latency cap. This is used to cap the contribution of 266 * latency compensation for stale signals, such as after the device has been 267 * disconnected from the CAN bus. 268 * @return Latency compensated value from the signal StatusSignal. 269 */ 270 public static double getLatencyCompensatedValueAsDouble(BaseStatusSignal signal, BaseStatusSignal signalSlope, double maxLatencySeconds) 271 { 272 final double nonCompensatedSignal = signal.getValueAsDouble(); 273 final double changeInSignal = signalSlope.getValueAsDouble(); 274 double latency = signal.getTimestamp().getLatency(); 275 if (maxLatencySeconds > 0.0 && latency > maxLatencySeconds) { 276 latency = maxLatencySeconds; 277 } 278 return nonCompensatedSignal + (changeInSignal * latency); 279 } 280 281 /** 282 * Checks if all signals have an OK error code. 283 * 284 * @param signals Signals to check error code of 285 * @return True if all are good, false otherwise 286 */ 287 public static boolean isAllGood(BaseStatusSignal... signals) { 288 for (BaseStatusSignal sig : signals) { 289 if (!sig.getStatus().isOK()) { 290 return false; 291 } 292 } 293 return true; 294 } 295 296 /** 297 * Sets the update frequency of all specified status signals to the provided common frequency. 298 * <p> 299 * A frequency of 0 Hz will turn off the signal. Otherwise, the minimum supported signal 300 * frequency is 4 Hz, and the maximum is 1000 Hz. 301 * <p> 302 * If other StatusSignals in the same status frame have been set to an update frequency, 303 * the fastest requested update frequency will be applied to the frame. 304 * <p> 305 * This will wait up to 0.100 seconds (100ms) for each signal. 306 * 307 * @param frequencyHz Rate to publish the signal in Hz 308 * @param signals Signals to apply the update frequency to 309 * @return Status code of the first failed update frequency set call, or OK if all succeeded 310 */ 311 public static StatusCode setUpdateFrequencyForAll(double frequencyHz, BaseStatusSignal... signals) { 312 StatusSignalJNI[] toSet = new StatusSignalJNI[signals.length]; 313 for (int i = 0; i < signals.length; ++i) { 314 toSet[i] = signals[i].jni; 315 } 316 return StatusCode.valueOf(StatusSignalJNI.JNI_SetUpdateFrequencyForAll(frequencyHz, toSet, 0.100)); 317 } 318 319 /** 320 * Sets the update frequency of all specified status signals to the provided common frequency. 321 * <p> 322 * A frequency of 0 Hz will turn off the signal. Otherwise, the minimum supported signal 323 * frequency is 4 Hz, and the maximum is 1000 Hz. 324 * <p> 325 * If other StatusSignals in the same status frame have been set to an update frequency, 326 * the fastest requested update frequency will be applied to the frame. 327 * <p> 328 * This will wait up to 0.100 seconds (100ms) for each signal. 329 * 330 * @param frequency Rate to publish the signal 331 * @param signals Signals to apply the update frequency to 332 * @return Status code of the first failed update frequency set call, or OK if all succeeded 333 */ 334 public static StatusCode setUpdateFrequencyForAll(Frequency frequency, BaseStatusSignal... signals) { 335 return setUpdateFrequencyForAll(frequency.in(Hertz), signals); 336 } 337 338 /** 339 * Sets the rate at which the device will publish this signal. 340 * <p> 341 * A frequency of 0 Hz will turn off the signal. Otherwise, the minimum supported signal 342 * frequency is 4 Hz, and the maximum is 1000 Hz. 343 * <p> 344 * If other StatusSignals in the same status frame have been set to an update frequency, 345 * the fastest requested update frequency will be applied to the frame. 346 * <p> 347 * This will wait up to 0.100 seconds (100ms) by default. 348 * 349 * @param frequencyHz Rate to publish the signal in Hz 350 * @return Status code of setting the update frequency 351 */ 352 public StatusCode setUpdateFrequency(double frequencyHz) { 353 return setUpdateFrequency(frequencyHz, 0.100); 354 } 355 356 /** 357 * Sets the rate at which the device will publish this signal. 358 * <p> 359 * A frequency of 0 Hz will turn off the signal. Otherwise, the minimum supported signal 360 * frequency is 4 Hz, and the maximum is 1000 Hz. 361 * <p> 362 * If other StatusSignals in the same status frame have been set to an update frequency, 363 * the fastest requested update frequency will be applied to the frame. 364 * <p> 365 * This will wait up to 0.100 seconds (100ms) by default. 366 * 367 * @param frequency Rate to publish the signal 368 * @return Status code of setting the update frequency 369 */ 370 public StatusCode setUpdateFrequency(Frequency frequency) { 371 return setUpdateFrequency(frequency.in(Hertz)); 372 } 373 374 /** 375 * Sets the rate at which the device will publish this signal. 376 * <p> 377 * A frequency of 0 Hz will turn off the signal. Otherwise, the minimum supported signal 378 * frequency is 4 Hz, and the maximum is 1000 Hz. 379 * <p> 380 * If other StatusSignals in the same status frame have been set to an update frequency, 381 * the fastest requested update frequency will be applied to the frame. 382 * 383 * @param frequencyHz Rate to publish the signal in Hz 384 * @param timeoutSeconds Maximum amount of time to wait when performing the action 385 * @return Status code of setting the update frequency 386 */ 387 public StatusCode setUpdateFrequency(double frequencyHz, double timeoutSeconds) { 388 return StatusCode.valueOf(jni.JNI_SetUpdateFrequency(frequencyHz, timeoutSeconds)); 389 } 390 391 /** 392 * Sets the rate at which the device will publish this signal. 393 * <p> 394 * A frequency of 0 Hz will turn off the signal. Otherwise, the minimum supported signal 395 * frequency is 4 Hz, and the maximum is 1000 Hz. 396 * <p> 397 * If other StatusSignals in the same status frame have been set to an update frequency, 398 * the fastest requested update frequency will be applied to the frame. 399 * 400 * @param frequency Rate to publish the signal 401 * @param timeoutSeconds Maximum amount of time to wait when performing the action 402 * @return Status code of setting the update frequency 403 */ 404 public StatusCode setUpdateFrequency(Frequency frequency, double timeoutSeconds) { 405 return setUpdateFrequency(frequency.in(Hertz), timeoutSeconds); 406 } 407 408 /** 409 * Gets the rate at which the device will publish this signal. 410 * <p> 411 * This is typically the last value passed into {@link #setUpdateFrequency}. The returned value 412 * may be higher if another StatusSignal in the same status frame has been set to a higher 413 * update frequency. 414 * 415 * @return Applied update frequency of the signal in Hz 416 */ 417 public double getAppliedUpdateFrequency() { 418 return jni.JNI_GetAppliedUpdateFrequency(); 419 } 420 421 /** 422 * Gets the rate at which the device will publish this signal as a unit type. 423 * <p> 424 * This is typically the last value passed into {@link #setUpdateFrequency}. The returned value 425 * may be higher if another StatusSignal in the same status frame has been set to a higher 426 * update frequency. 427 * 428 * @return Applied update frequency of the signal 429 */ 430 public Frequency getAppliedUpdateFrequencyMeasure() { 431 return Hertz.of(getAppliedUpdateFrequency()); 432 } 433 434 /** 435 * Gets the name of this signal. 436 * 437 * @return Name of this signal 438 */ 439 public String getName() { 440 return signalName; 441 } 442 443 /** 444 * Gets the units for this signal. 445 * 446 * @return String representation of units for this signal 447 */ 448 public String getUnits() { 449 return units; 450 } 451 452 /** 453 * Gets the value of this signal as a double. 454 * 455 * @return Value of this signal as a double instead of the generic type 456 */ 457 public double getValueAsDouble() { 458 return baseValue; 459 } 460 461 /** 462 * Get all timestamps relevant for this signal. 463 * 464 * @return All timestamps available for this signal 465 */ 466 public AllTimestamps getAllTimestamps() { 467 return timestamps; 468 } 469 470 /** 471 * Get the most accurate timestamp available for this signal. 472 * <p> 473 * The timestamp sources from most to least accurate are: 474 * <ul> 475 * <li> {@link Timestamp.TimestampSource#Device} 476 * <li> {@link Timestamp.TimestampSource#CANivore} 477 * <li> {@link Timestamp.TimestampSource#System} 478 * </ul> 479 * Note that some of these sources may not be available. 480 * 481 * @return The most accurate timestamp available 482 */ 483 public Timestamp getTimestamp() { 484 return timestamps.getBestTimestamp(); 485 } 486 487 /** 488 * Get the error code from when we last received this signal. 489 * 490 * @return Last cached Error Code 491 */ 492 public StatusCode getStatus() { 493 return error; 494 } 495 496 /** 497 * Check whether the signal has been updated since the last check. 498 * <p> 499 * Note that the signal must be refreshed before calling this routine. 500 * 501 * @return true if the signal has updated since the previous call of this routine 502 */ 503 public boolean hasUpdated() 504 { 505 boolean retval = false; 506 /* did we receive an update */ 507 final var timestamp = getAllTimestamps().getSystemTimestamp(); 508 if (timestamp.isValid()) { 509 /* if the update timestamp is new, then a new frame was sent */ 510 if (_lastTimestamp != timestamp.getTime()) { 511 _lastTimestamp = timestamp.getTime(); 512 retval = true; 513 } 514 } 515 return retval; 516 } 517};