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};