agrobot_base/Firmware/Modulos/SensorFluxoModel_v2.h

#include "SerialService.h"
#include "Pinout.h"
#include "IComponenteCAN.h"

#ifndef SensorFluxoModel_v2_h
#define SensorFluxoModel_v2_h

class SensorFluxo_v2 : public ComponenteCAN {
  public:
    SensorFluxo_v2(
      const String& id,
      uint8_t idNum,
      int pino,
      TiposBarramentos barramento = CONTROLADOR
    )
    : _ID(id),
      ID_Num(idNum),
      _pino(
        pino,
        barramento,
        _INPUT_PULLUP,
        DIGITAL
      ) {
    }

    String _ID;
    uint8_t ID_Num = 0;
    PinoModel _pino;

    bool Iniciado = false;

    /*
     * Calibração do sensor.
     *
     * Equação original:
     * Q (L/min) = (frequência + 3) / 8.1
     */
    double FatorCalibracao = 0.776;
    double VazaoMaximaLMin = 10.0;

    /*
     * Após esse tempo sem nenhum pulso válido,
     * consideramos que não existe fluxo.
     */
    unsigned long TimeoutSemPulsoMs = 700;

    /*
     * Ignora pulsos rápidos demais para serem reais.
     *
     * 3 ms permite até aproximadamente 333 Hz,
     * muito acima da frequência esperada para 10 L/min.
     */
    unsigned long IntervaloMinimoPulsoMs = 3;

    /*
     * Valores consolidados para telemetria CAN.
     */
    unsigned int _PulsosLeitura = 0;
    unsigned long _PeriodoTotalLeitura = 0;
    float _Vazao = 0.0f;
    bool _FluxoPresente = false;
    unsigned long _UltimoPulsoMs = 0;

    /*
     * Protege aquisição, janela de controle e telemetria.
     */
    portMUX_TYPE _stateMux = portMUX_INITIALIZER_UNLOCKED;

    void Inicializar() {
      if (Iniciado) {
        MostrarLog("Sensor ja inicializado");
        return;
      }

      _pino.Conectar();
      Iniciado = _pino.Definido();

      if (!Iniciado) {
        MostrarLog("Sensor nao iniciado");
        return;
      }

      ResetarEstado();

      MostrarLog("Sensor iniciado");
    }

    void Desligar() {
      if (!Iniciado) {
        MostrarLog("Sensor nao esta inicializado");
        return;
      }

      ResetarEstado();
      _pino.Desconectar();

      Iniciado = false;

      MostrarLog("Sensor desligado");
    }

    /*
     * Deve continuar sendo chamado no loop rápido.
     *
     * Detecta somente a borda de subida do sinal.
     */
    void Loop() {
      if (!Iniciado)
        return;

      bool leituraAtual = (_pino.get() == 1);
      unsigned long agora = millis();

      portENTER_CRITICAL(&_stateMux);

      bool mudouEstado =
        leituraAtual != _ultimaLeituraDigital;

      if (mudouEstado) {
        bool bordaSubida =
          leituraAtual &&
          !_ultimaLeituraDigital;

        if (bordaSubida) {
          RegistrarPulsoInterno(agora);
        }

        _ultimaLeituraDigital = leituraAtual;
      }

      portEXIT_CRITICAL(&_stateMux);
    }

    /*
     * Atualiza somente o snapshot destinado à telemetria.
     *
     * Não interfere na janela utilizada pelo controle.
     */
    void RequisitarDados() {
      AtualizarLeituraTelemetria();
    }

    std::vector<uint8_t> MontarMensagemCAN(
      CanMessagePosicaoDados posicao
    ) {
      std::vector<uint8_t> data;

      data.push_back(
        static_cast<uint8_t>(posicao)
      );

      data.push_back(ID_Num);

      switch (posicao) {
        case CanMessagePosicaoDados::Status: {
          data.push_back(Iniciado ? 1 : 0);
          break;
        }

        case CanMessagePosicaoDados::Dados1: {
          unsigned int pulsosLeitura;
          unsigned long periodoLeitura;
          float vazao;

          portENTER_CRITICAL(&_stateMux);

          pulsosLeitura = _PulsosLeitura;
          periodoLeitura = _PeriodoTotalLeitura;
          vazao = _Vazao;

          portEXIT_CRITICAL(&_stateMux);

          /*
           * O protocolo atual possui:
           *
           * pulsos: 1 byte
           * período: 2 bytes
           * vazão: 2 bytes, escala ×100
           */
          uint8_t pulsosEscalados =
            pulsosLeitura > 255
              ? 255
              : (uint8_t)pulsosLeitura;

          uint16_t periodoEscalado =
            periodoLeitura > 65535UL
              ? 65535
              : (uint16_t)periodoLeitura;

          double vazaoEscaladaDouble =
            round((double)vazao * 100.0);

          if (vazaoEscaladaDouble < 0.0)
            vazaoEscaladaDouble = 0.0;

          if (vazaoEscaladaDouble > 65535.0)
            vazaoEscaladaDouble = 65535.0;

          uint16_t vazaoEscalada =
            (uint16_t)vazaoEscaladaDouble;

          data.push_back(pulsosEscalados);

          data.push_back(
            (uint8_t)(periodoEscalado >> 8)
          );

          data.push_back(
            (uint8_t)(periodoEscalado & 0xFF)
          );

          data.push_back(
            (uint8_t)(vazaoEscalada >> 8)
          );

          data.push_back(
            (uint8_t)(vazaoEscalada & 0xFF)
          );

          break;
        }

        default:
          break;
      }

      return data;
    }

    /*
     * Retorna a janela corrente da telemetria,
     * sem reiniciá-la.
     *
     * Isso serve para diagnóstico. A bomba não deve
     * usar essa janela para controle.
     */
    void ObterAmostragemAtual(
      unsigned int& pulsos,
      unsigned long& periodoMs
    ) {
      unsigned long agora = millis();

      portENTER_CRITICAL(&_stateMux);

      pulsos = _PulsosTelemetria;

      periodoMs =
        _InicioJanelaTelemetriaMs > 0
          ? agora - _InicioJanelaTelemetriaMs
          : 0;

      portEXIT_CRITICAL(&_stateMux);
    }

    /*
     * Snapshot somente-leitura para a malha da bomba.
     *
     * Usa uma janela circular própria de intervalos.
     * Uma requisição CAN nunca reinicia essa janela.
     */
    void ObterSnapshotControle(
      float& vazaoMLs,
      bool& fluxoPresente,
      unsigned long& ultimoPulsoMs
    ) {
      unsigned long somaIntervalosMs;
      uint8_t quantidadeIntervalos;
      unsigned long ultimoPulso;

      portENTER_CRITICAL(&_stateMux);

      somaIntervalosMs =
        _SomaIntervalosControleMs;

      quantidadeIntervalos =
        _QuantidadeIntervalosControle;

      ultimoPulso =
        _UltimoPulsoMs;

      portEXIT_CRITICAL(&_stateMux);

      unsigned long agora = millis();

      bool pulsoRecente =
        ultimoPulso > 0 &&
        (agora - ultimoPulso) <= TimeoutSemPulsoMs;

      /*
       * Um pulso recente já confirma presença física
       * de fluxo, mesmo que ainda não existam dois
       * pulsos para calcular sua frequência.
       */
      fluxoPresente = pulsoRecente;
      ultimoPulsoMs = ultimoPulso;

      if (
        !pulsoRecente ||
        quantidadeIntervalos == 0 ||
        somaIntervalosMs == 0
      ) {
        vazaoMLs = 0.0f;
        return;
      }

      double periodoMedioMs =
        (double)somaIntervalosMs /
        (double)quantidadeIntervalos;

      double frequenciaHz =
        1000.0 / periodoMedioMs;

      vazaoMLs =
        CalcularVazaoPorFrequencia(
          frequenciaHz
        );
    }

    /*
     * Conversão única e oficial da frequência para
     * vazão corrigida.
     */
    float CalcularVazaoPorFrequencia(
      double frequenciaHz
    ) const {
      if (
        !isfinite(frequenciaHz) ||
        frequenciaHz <= 0.0
      ) {
        return 0.0f;
      }

      double vazaoLMin =
        (frequenciaHz + 3.0) / 8.1;

      if (vazaoLMin < 0.0)
        vazaoLMin = 0.0;

      if (vazaoLMin > VazaoMaximaLMin)
        vazaoLMin = VazaoMaximaLMin;

      double vazaoMLs =
        vazaoLMin *
        (1000.0 / 60.0) *
        FatorCalibracao;

      if (
        !isfinite(vazaoMLs) ||
        vazaoMLs < 0.0
      ) {
        return 0.0f;
      }

      return (float)vazaoMLs;
    }

    float CalcularVazao(
      unsigned int pulsos,
      unsigned long periodoMs
    ) const {
      if (
        pulsos == 0 ||
        periodoMs == 0
      ) {
        return 0.0f;
      }

      double frequenciaHz =
        ((double)pulsos * 1000.0) /
        (double)periodoMs;

      return CalcularVazaoPorFrequencia(
        frequenciaHz
      );
    }

  private:
    bool DebugMode = false;

    /*
     * Quantidade de intervalos recentes utilizada
     * pelo controle da bomba.
     *
     * Quatro intervalos oferecem um bom compromisso:
     * - reduz oscilação;
     * - não torna a resposta lenta demais com um bico.
     */
    static const uint8_t TAMANHO_JANELA_CONTROLE = 4;

    unsigned long
      _IntervalosControleMs[TAMANHO_JANELA_CONTROLE] = {
        0, 0, 0, 0
      };

    uint8_t _IndiceIntervaloControle = 0;
    uint8_t _QuantidadeIntervalosControle = 0;
    unsigned long _SomaIntervalosControleMs = 0;

    /*
     * Janela exclusiva da telemetria.
     *
     * Somente essa janela é zerada por
     * AtualizarLeituraTelemetria().
     */
    unsigned int _PulsosTelemetria = 0;
    unsigned long _InicioJanelaTelemetriaMs = 0;

    /*
     * Estado comum de aquisição.
     */
    bool _ultimaLeituraDigital = false;
    unsigned long _UltimoPulsoValidoMs = 0;

    void RegistrarPulsoInterno(
      unsigned long agora
    ) {
      /*
       * Este método é chamado com o mux travado.
       */
      if (_UltimoPulsoValidoMs > 0) {
        unsigned long intervalo =
          agora - _UltimoPulsoValidoMs;

        /*
         * Ignora bounce ou ruído rápido.
         *
         * Não atualizamos o timestamp do último
         * pulso válido quando o pulso é rejeitado.
         */
        if (intervalo < IntervaloMinimoPulsoMs) {
          return;
        }

        AdicionarIntervaloControle(
          intervalo
        );
      }

      _UltimoPulsoValidoMs = agora;
      _UltimoPulsoMs = agora;

      if (_PulsosTelemetria < UINT_MAX) {
        _PulsosTelemetria++;
      }
    }

    void AdicionarIntervaloControle(
      unsigned long intervaloMs
    ) {
      /*
       * Este método também é chamado com o mux travado.
       */
      if (
        _QuantidadeIntervalosControle <
        TAMANHO_JANELA_CONTROLE
      ) {
        _IntervalosControleMs[
          _IndiceIntervaloControle
        ] = intervaloMs;

        _SomaIntervalosControleMs +=
          intervaloMs;

        _QuantidadeIntervalosControle++;
      } else {
        _SomaIntervalosControleMs -=
          _IntervalosControleMs[
            _IndiceIntervaloControle
          ];

        _IntervalosControleMs[
          _IndiceIntervaloControle
        ] = intervaloMs;

        _SomaIntervalosControleMs +=
          intervaloMs;
      }

      _IndiceIntervaloControle =
        (
          _IndiceIntervaloControle + 1
        ) %
        TAMANHO_JANELA_CONTROLE;
    }

    void AtualizarLeituraTelemetria() {
      if (!Iniciado)
        return;

      unsigned long agora = millis();

      unsigned int pulsos;
      unsigned long periodoMs;

      unsigned long somaIntervalosControle;
      uint8_t quantidadeIntervalosControle;
      unsigned long ultimoPulso;

      portENTER_CRITICAL(&_stateMux);

      pulsos = _PulsosTelemetria;

      periodoMs =
        _InicioJanelaTelemetriaMs > 0
          ? agora - _InicioJanelaTelemetriaMs
          : 0;

      /*
       * Reinicia somente a janela de telemetria.
       */
      _PulsosTelemetria = 0;
      _InicioJanelaTelemetriaMs = agora;

      /*
       * A janela de controle é copiada, mas nunca
       * alterada ou reiniciada aqui.
       */
      somaIntervalosControle =
        _SomaIntervalosControleMs;

      quantidadeIntervalosControle =
        _QuantidadeIntervalosControle;

      ultimoPulso =
        _UltimoPulsoMs;

      portEXIT_CRITICAL(&_stateMux);

      bool pulsoRecente =
        ultimoPulso > 0 &&
        (agora - ultimoPulso) <= TimeoutSemPulsoMs;

      float vazao = 0.0f;

      /*
       * Para a telemetria, usamos a média da janela
       * de aquisição desde a última requisição.
       */
      if (
        pulsoRecente &&
        pulsos > 0 &&
        periodoMs > 0
      ) {
        vazao =
          CalcularVazao(
            pulsos,
            periodoMs
          );
      } else if (
        pulsoRecente &&
        quantidadeIntervalosControle > 0 &&
        somaIntervalosControle > 0
      ) {
        /*
         * Fallback:
         *
         * Se a janela CAN foi curta e ainda não recebeu
         * pulso, usa a janela circular do controle.
         */
        double periodoMedioMs =
          (double)somaIntervalosControle /
          (double)quantidadeIntervalosControle;

        double frequenciaHz =
          1000.0 / periodoMedioMs;

        vazao =
          CalcularVazaoPorFrequencia(
            frequenciaHz
          );
      }

      portENTER_CRITICAL(&_stateMux);

      _PulsosLeitura = pulsos;
      _PeriodoTotalLeitura = periodoMs;
      _Vazao = vazao;
      _FluxoPresente = pulsoRecente;

      portEXIT_CRITICAL(&_stateMux);
    }

    void ResetarEstado() {
      bool leituraInicial = false;

      if (_pino.Definido()) {
        leituraInicial =
          (_pino.get() == 1);
      }

      unsigned long agora = millis();

      portENTER_CRITICAL(&_stateMux);

      _ultimaLeituraDigital =
        leituraInicial;

      _UltimoPulsoValidoMs = 0;
      _UltimoPulsoMs = 0;

      /*
       * Limpa janela de controle.
       */
      for (
        uint8_t i = 0;
        i < TAMANHO_JANELA_CONTROLE;
        i++
      ) {
        _IntervalosControleMs[i] = 0;
      }

      _IndiceIntervaloControle = 0;
      _QuantidadeIntervalosControle = 0;
      _SomaIntervalosControleMs = 0;

      /*
       * Limpa janela de telemetria.
       */
      _PulsosTelemetria = 0;
      _InicioJanelaTelemetriaMs = agora;

      /*
       * Limpa snapshot publicado.
       */
      _PulsosLeitura = 0;
      _PeriodoTotalLeitura = 0;
      _Vazao = 0.0f;
      _FluxoPresente = false;

      portEXIT_CRITICAL(&_stateMux);
    }

    void MostrarLog(
      String mensagem
    ) {
      if (DebugMode) {
        PrintTela(
          "[FLX " +
          _ID +
          "] " +
          mensagem
        );
      }
    }
};

#endif