271 lines
8.5 KiB
C++
271 lines
8.5 KiB
C++
#ifndef SensorCorrenteModel
|
|
#define SensorCorrenteModel
|
|
|
|
#include "SerialService.h"
|
|
#include "I2CService.h"
|
|
#include "Utils.h"
|
|
#include <Wire.h>
|
|
#include <vector>
|
|
#include <Adafruit_INA219.h>
|
|
#include <SimpleKalmanFilter.h>
|
|
|
|
class SensorCorrente {
|
|
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 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 {
|
|
Iniciado = true;
|
|
switch (_A_Shunt) {
|
|
case TiposShuntCorrente::INA228_10A:
|
|
currentLSB = currentLSB_10A;
|
|
break;
|
|
case TiposShuntCorrente::INA228_50A:
|
|
currentLSB = currentLSB_50A;
|
|
break;
|
|
case TiposShuntCorrente::INA228_100A:
|
|
currentLSB = currentLSB_100A;
|
|
break;
|
|
}
|
|
EscreverRegistradorINA(0x00, 0x8000); // Bit 15 = 1 → RESET
|
|
uint16_t cal = (uint16_t)(0.00512 / (currentLSB * shuntLSB));
|
|
//EscreverRegistradorINA(0xD4, cal);
|
|
MostrarLog("Sensor (INA228) iniciado no canal " + String(_CanalMUX));
|
|
}
|
|
} 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);
|
|
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;
|
|
static constexpr float currentLSB_10A = 0.0001;
|
|
static constexpr float currentLSB_50A = 0.01367;
|
|
static constexpr float currentLSB_100A = 0.0243;
|
|
static constexpr float shuntLSB = 1.0e-6;
|
|
static constexpr float vBusLSB = 1.23e-5;
|
|
float currentLSB = 0.0001;
|
|
|
|
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;
|
|
}
|
|
|
|
void EscreverRegistradorINA(uint8_t reg, uint16_t valor) {
|
|
uint8_t dados[3];
|
|
dados[0] = reg;
|
|
dados[1] = (valor >> 8) & 0xFF;
|
|
dados[2] = valor & 0xFF;
|
|
|
|
if (!I2CService::EscritaSegura(_Endereco, dados, 3, 50)) {
|
|
Serial.printf("[INA] Falha ao escrever no registrador 0x%02X\n", reg);
|
|
}
|
|
}
|
|
|
|
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 {
|
|
uint32_t rawBus = LerRegistradorINA(0x05, 3);
|
|
uint32_t rawCurrent = LerRegistradorINA(0x07, 3);
|
|
uint32_t rawPower = LerRegistradorINA(0x08, 3);
|
|
uint64_t rawEnergy = ((uint64_t)LerRegistradorINA(0x0A, 3) << 16) | LerRegistradorINA(0x0B, 2);
|
|
uint16_t rawTemp = LerRegistradorINA(0x0F, 2);
|
|
|
|
float K = 50.58;
|
|
|
|
busVoltage = rawBus * vBusLSB; // V
|
|
power = rawPower * currentLSB * K; // mW
|
|
float _current = rawCurrent * currentLSB; // mA
|
|
current = filtro.updateEstimate(_current); // mA
|
|
energy = rawEnergy * currentLSB * K * 16 / 3600.0 / 1000.0; // Wh
|
|
temperature = ((int16_t)rawTemp) * 0.0078125; // C
|
|
|
|
MostrarLog("BusVoltage: " + String(busVoltage) + ", Current: " + String(current) + ", Power: " + String(power) + ", Energy: " + String(energy) + ", Temperature: " + String(temperature));
|
|
}
|
|
|
|
I2CService::LiberarAcessoI2C(ID_Num);
|
|
}
|
|
};
|
|
|
|
#endif |