001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.vision.pipe.impl;
019
020import edu.wpi.first.math.filter.LinearFilter;
021import edu.wpi.first.wpilibj.Timer;
022import org.photonvision.vision.pipe.CVPipe;
023
024public class CalculateFPSPipe
025        extends CVPipe<Void, Integer, CalculateFPSPipe.CalculateFPSPipeParams> {
026    private final LinearFilter fpsFilter = LinearFilter.movingAverage(20);
027
028    // roll my own Timer, since this is so trivial
029    double lastTime = -1;
030
031    @Override
032    protected Integer process(Void in) {
033        if (lastTime < 0) {
034            lastTime = Timer.getFPGATimestamp();
035        }
036
037        var now = Timer.getFPGATimestamp();
038        var dtSeconds = now - lastTime;
039        lastTime = now;
040
041        // If < 1 uS between ticks, something is probably wrong
042        int fps;
043        if (dtSeconds < 1e-6) {
044            fps = 0;
045        } else {
046            fps = (int) fpsFilter.calculate(1 / dtSeconds);
047        }
048
049        return fps;
050    }
051
052    public static class CalculateFPSPipeParams {}
053}