agrobot_base/Firmware/Modulos/SensorCorrenteModel.h

#ifndef SensorCorrenteModel
#define SensorCorrenteModel

#include "SerialService.h"
#include "I2CService.h"
#include "Utils.h"
#include "IComponenteCAN.h"
#include <Wire.h>
#include <vector>
#include <Adafruit_INA219.h>
#include <Adafruit_INA228.h>
#include <SimpleKalmanFilter.h>

class SensorCorrente : public ComponenteCAN {
  public:
    SensorCorrente(String _modID, String _id) {
      Mod_ID = _modID;
      _ID = _id;
    }

    String Mod_ID = "";
    String _ID;
    int ID_Num;
    byte _Endereco;
    TiposShuntCorrente _A_Shunt = TiposShuntCorrente::INA228_10A;
    int _CanalMUX;
    bool isINA219 = false;

    bool Iniciado = false;
    float busVoltage = 0;
    float current = 0;
    float power = 0;
    float energy = 0;
    float charge = 0;
    uint16_t codAlarme = 0;
    float temperature = 0;

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

      if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) return;

      isINA219 = _A_Shunt == TiposShuntCorrente::INA219_3A2;

      bool encontrado = I2CService::VerificaEnderecoBarramento(_Endereco);
      if (encontrado) {
        if (isINA219) {
          sINA219 = Adafruit_INA219(_Endereco);
          Iniciado = sINA219.begin();
          if (Iniciado) {
            sINA219.setCalibration_32V_2A();
            MostrarLog("Sensor (INA219) iniciado no canal " + String(_CanalMUX));
          }
          else {
            PrintTela("Sensor nao iniciado!");
          }
        }
        else {
          sINA228 = Adafruit_INA228();
          Iniciado = sINA228.begin(_Endereco);
          if (Iniciado) {
            if (_A_Shunt == TiposShuntCorrente::INA228_10A) {
              sINA228.setShunt(0.015, 10.0);
            }
            else if (_A_Shunt == TiposShuntCorrente::INA228_50A) {
              sINA228.setShunt(0.0015, 50.0);
            }
            else if (_A_Shunt == TiposShuntCorrente::INA228_100A) {
              sINA228.setShunt(0.00075, 100.0);
            }
            sINA228.setAveragingCount(INA228_COUNT_16);
            sINA228.setCurrentConversionTime(INA228_TIME_280_us);
            MostrarLog("Sensor (INA219) iniciado no canal " + String(_CanalMUX));
          }
          else {
            PrintTela("Sensor nao iniciado!");
          }
        }        
      } else {
        Iniciado = false;
        MostrarLog("Sensor nao iniciado, endereco " + String(_Endereco) + " nao encontrado!");
      }
    
      I2CService::LiberarAcessoI2C(ID_Num);
    }

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

      Iniciado = false;

      if (I2CService::QuemEstaUsando() == ID_Num) {
        while (I2CService::QuemEstaUsando() == ID_Num) {
          vTaskDelay(10);
        }
      }
      
      MostrarLog("Sensor Desligado");
    }

    void RequisitarDados() {
      if (!Iniciado) return;
      AferirCorrente();
    }

    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: {
          uint16_t volts = busVoltage * 100;
          uint16_t amps = current / 10;
          uint16_t watts = power / 10;
          data.push_back(volts >> 8); data.push_back(volts & 0xFF);
          data.push_back(amps >> 8); data.push_back(amps & 0xFF);
          data.push_back(watts >> 8); data.push_back(watts & 0xFF);
          break;
        }
        case CanMessagePosicaoDados::Dados2: {
          if (isINA219) break;
          uint16_t wh = energy / 10;
          uint16_t temp = temperature * 100;
          data.push_back(wh >> 8); data.push_back(wh & 0xFF);
          data.push_back(temp >> 8); data.push_back(temp & 0xFF);
          data.push_back(codAlarme >> 8); data.push_back(codAlarme & 0xFF);
          break;
        }
      }
      return data;
    }

    static std::vector<uint8_t> ConfigurarSensor(std::vector<SensorCorrente*>& lista, std::vector<uint8_t>& data, const String& Mod_ID) {
      std::vector<uint8_t> status;
      if (data.size() < 2) return status;
      CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
      uint8_t idNum = data[1];
      auto it = std::find_if(lista.begin(), lista.end(), [idNum](SensorCorrente* s) { return s->ID_Num == idNum; });
      bool jaExiste = it != lista.end();
      SensorCorrente* sensor;

      switch (posicao) {
        case CanMessagePosicaoDados::Config1: {
          if (data.size() < 7) return status;
          bool conectar = data[3] == 1;
          uint8_t endereco = data[4];
          TiposShuntCorrente aMax = (TiposShuntCorrente)data[5];
          int canalMux = (data[6] == 255) ? -1 : data[6];

          if (conectar) {
            if (!jaExiste) {
              sensor = new SensorCorrente(Mod_ID, "sCOR_" + String(idNum));
            } else {
              sensor = *it;
            }
            if (!sensor->Iniciado) {
              sensor->ID_Num = idNum;
              sensor->_Endereco = endereco;
              sensor->_A_Shunt = aMax;
              sensor->_CanalMUX = canalMux;
              sensor->Inicializar();
              if (!jaExiste) {
                lista.push_back(sensor);
                sensor->MostrarLog("Sensor de corrente adicionado via CAN: sCOR_" + String(idNum));
              }
            }
            status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
          } else {
            if (jaExiste) {
              sensor = *it;
              sensor->Desligar();
              status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
              sensor->MostrarLog("Sensor de corrente removido via CAN: sCOR_" + String(idNum));
              delete sensor;
              lista.erase(it);
            }
          }
          break;
        }
      }
      return status;
    }

  private:

    bool DebugMode = false;
    void MostrarLog(String mensagem) {
      if (DebugMode) {
        PrintTela("[COR " + _ID + "] " + mensagem);
      }
    }

    SimpleKalmanFilter filtro = SimpleKalmanFilter(10, 10, 0.05);
    Adafruit_INA219 sINA219;
    Adafruit_INA228 sINA228;

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

      if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) return;

      if (isINA219) {
        busVoltage = sINA219.getBusVoltage_V(); // V
        float _current = sINA219.getCurrent_mA(); // mA
        current = filtro.updateEstimate(_current); // mA
        power = sINA219.getPower_mW(); //mW

        MostrarLog("BusVoltage: " + String(busVoltage) + ", Current: " + String(current) + ", Power: " + String(power));
      }
      else {
        busVoltage = sINA228.getBusVoltage_V(); // V
        float _current = sINA228.getCurrent_mA(); // mA
        current = filtro.updateEstimate(_current); // mA
        power = sINA228.getPower_mW(); //mW
        temperature = sINA228.readDieTemp(); //C
        energy = sINA228.readEnergy(); //Wh
        charge = sINA228.readCharge(); //Ah
        codAlarme = LerRegistradorINA(0x0B, 2);

        MostrarLog("BusVoltage: " + String(busVoltage) + ", Current: " + String(current) + ", Power: " + String(power) + ", Energy: " + String(energy) + ", Temperature: " + String(temperature));
      }

      I2CService::LiberarAcessoI2C(ID_Num);
    }

    uint32_t LerRegistradorINA(uint8_t reg, uint8_t bytes) {
      uint8_t buffer[4] = {0};
  
      if (bytes > 4) return 0;
  
      // Aumentar timeout para garantir tempo de resposta
      if (!I2CService::LeituraSegura(_Endereco, buffer, bytes, reg)) {
        Serial.printf("[sCOR] Falha na leitura do registrador 0x%02X\n", reg);
        return 0;
      }
  
      uint32_t valor = 0;
      for (uint8_t i = 0; i < bytes; i++) {
        valor = (valor << 8) | buffer[i];
      }
  
      return valor;
    }

  };

#endif