394 lines
11 KiB
C++
394 lines
11 KiB
C++
#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>
|
|
|
|
#ifndef SensorCorrenteModel_v2
|
|
#define SensorCorrenteModel_v2
|
|
|
|
class SensorCorrente_v2 : public ComponenteCAN {
|
|
public:
|
|
String ID;
|
|
uint8_t ID_Num = 0;
|
|
uint8_t _Endereco = 0;
|
|
TiposShuntCorrente _A_Shunt = TiposShuntCorrente::INA228_10A;
|
|
int _CanalMUX = -1;
|
|
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;
|
|
|
|
SensorCorrente_v2() {}
|
|
|
|
SensorCorrente_v2(
|
|
const String& id,
|
|
uint8_t idNum,
|
|
uint8_t endereco,
|
|
TiposShuntCorrente tipoShunt,
|
|
int canalMux = -1
|
|
) :
|
|
ID(id),
|
|
ID_Num(idNum),
|
|
_Endereco(endereco),
|
|
_A_Shunt(tipoShunt),
|
|
_CanalMUX(canalMux)
|
|
{
|
|
}
|
|
|
|
bool EstaIniciado() {
|
|
bool iniciado;
|
|
portENTER_CRITICAL(&_snapshotLock);
|
|
iniciado = Iniciado;
|
|
portEXIT_CRITICAL(&_snapshotLock);
|
|
return iniciado;
|
|
}
|
|
|
|
uint8_t FalhasConsecutivas() {
|
|
uint8_t falhas;
|
|
portENTER_CRITICAL(&_snapshotLock);
|
|
falhas = _falhasConsecutivas;
|
|
portEXIT_CRITICAL(&_snapshotLock);
|
|
return falhas;
|
|
}
|
|
|
|
uint32_t TotalFalhasLeitura() {
|
|
uint32_t total;
|
|
portENTER_CRITICAL(&_snapshotLock);
|
|
total = _totalFalhasLeitura;
|
|
portEXIT_CRITICAL(&_snapshotLock);
|
|
return total;
|
|
}
|
|
|
|
void Inicializar() {
|
|
if (EstaIniciado()) {
|
|
MostrarLog("Sensor ja inicializado");
|
|
return;
|
|
}
|
|
|
|
if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) {
|
|
MostrarLog("Falha ao solicitar acesso I2C");
|
|
return;
|
|
}
|
|
|
|
isINA219 = (_A_Shunt == TiposShuntCorrente::INA219_3A2);
|
|
|
|
bool encontrado = I2CService::VerificaEnderecoBarramento(_Endereco);
|
|
if (!encontrado) {
|
|
AtualizarEstadoIniciado(false);
|
|
MostrarLog("Endereco I2C nao encontrado: " + String(_Endereco));
|
|
I2CService::LiberarAcessoI2C(ID_Num);
|
|
return;
|
|
}
|
|
|
|
if (isINA219) {
|
|
sINA219 = Adafruit_INA219(_Endereco);
|
|
bool iniciou = sINA219.begin();
|
|
AtualizarEstadoIniciado(iniciou);
|
|
|
|
if (iniciou) {
|
|
sINA219.setCalibration_32V_2A();
|
|
MostrarLog("INA219 iniciado no canal " + String(_CanalMUX));
|
|
} else {
|
|
MostrarLog("Falha ao iniciar INA219");
|
|
}
|
|
}
|
|
else {
|
|
sINA228 = Adafruit_INA228();
|
|
bool iniciou = sINA228.begin(_Endereco);
|
|
AtualizarEstadoIniciado(iniciou);
|
|
|
|
if (iniciou) {
|
|
ConfigurarShuntINA228();
|
|
sINA228.setAveragingCount(INA228_COUNT_16);
|
|
sINA228.setCurrentConversionTime(INA228_TIME_280_us);
|
|
MostrarLog("INA228 iniciado no canal " + String(_CanalMUX));
|
|
} else {
|
|
MostrarLog("Falha ao iniciar INA228");
|
|
}
|
|
}
|
|
|
|
I2CService::LiberarAcessoI2C(ID_Num);
|
|
|
|
if (EstaIniciado()) {
|
|
portENTER_CRITICAL(&_snapshotLock);
|
|
_falhasConsecutivas = 0;
|
|
portEXIT_CRITICAL(&_snapshotLock);
|
|
}
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!EstaIniciado()) {
|
|
MostrarLog("Sensor nao esta inicializado");
|
|
return;
|
|
}
|
|
|
|
AtualizarEstadoIniciado(false);
|
|
MostrarLog("Sensor desligado");
|
|
}
|
|
|
|
void RequisitarDados() {
|
|
if (!EstaIniciado()) 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);
|
|
|
|
bool iniciado;
|
|
bool ina219;
|
|
float snapBusVoltage;
|
|
float snapCurrent;
|
|
float snapPower;
|
|
float snapEnergy;
|
|
uint16_t snapCodAlarme;
|
|
float snapTemperature;
|
|
|
|
// Copia curta e atômica. Serialização acontece fora da critical section.
|
|
portENTER_CRITICAL(&_snapshotLock);
|
|
iniciado = Iniciado;
|
|
ina219 = isINA219;
|
|
snapBusVoltage = busVoltage;
|
|
snapCurrent = current;
|
|
snapPower = power;
|
|
snapEnergy = energy;
|
|
snapCodAlarme = codAlarme;
|
|
snapTemperature = temperature;
|
|
portEXIT_CRITICAL(&_snapshotLock);
|
|
|
|
switch (posicao) {
|
|
case CanMessagePosicaoDados::Status: {
|
|
data.push_back(iniciado ? 1 : 0);
|
|
break;
|
|
}
|
|
|
|
case CanMessagePosicaoDados::Dados1: {
|
|
uint16_t volts = (uint16_t)(snapBusVoltage * 100.0f);
|
|
uint16_t amps = (uint16_t)(snapCurrent / 10.0f);
|
|
uint16_t watts = (uint16_t)(snapPower / 10.0f);
|
|
|
|
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 (ina219) break;
|
|
|
|
uint16_t wh = (uint16_t)(snapEnergy / 10.0f);
|
|
uint16_t temp = (uint16_t)(snapTemperature * 100.0f);
|
|
|
|
data.push_back(wh >> 8); data.push_back(wh & 0xFF);
|
|
data.push_back(temp >> 8); data.push_back(temp & 0xFF);
|
|
data.push_back(snapCodAlarme >> 8); data.push_back(snapCodAlarme & 0xFF);
|
|
break;
|
|
}
|
|
}
|
|
|
|
return data;
|
|
}
|
|
|
|
private:
|
|
bool DebugMode = false;
|
|
SimpleKalmanFilter filtro = SimpleKalmanFilter(10, 10, 0.05);
|
|
Adafruit_INA219 sINA219;
|
|
Adafruit_INA228 sINA228;
|
|
portMUX_TYPE _snapshotLock = portMUX_INITIALIZER_UNLOCKED;
|
|
uint8_t _falhasConsecutivas = 0;
|
|
uint32_t _totalFalhasLeitura = 0;
|
|
static constexpr uint8_t LimiteFalhasAntesReinit = 3;
|
|
|
|
void AtualizarEstadoIniciado(bool iniciado) {
|
|
portENTER_CRITICAL(&_snapshotLock);
|
|
Iniciado = iniciado;
|
|
portEXIT_CRITICAL(&_snapshotLock);
|
|
}
|
|
|
|
void RegistrarFalhaAquisicao(const String& motivo) {
|
|
uint8_t falhas;
|
|
|
|
portENTER_CRITICAL(&_snapshotLock);
|
|
if (_falhasConsecutivas < UINT8_MAX) {
|
|
_falhasConsecutivas++;
|
|
}
|
|
_totalFalhasLeitura++;
|
|
falhas = _falhasConsecutivas;
|
|
portEXIT_CRITICAL(&_snapshotLock);
|
|
|
|
MostrarLog(
|
|
"Falha de aquisicao " + String(falhas) + "/" +
|
|
String(LimiteFalhasAntesReinit) + " | " + motivo
|
|
);
|
|
|
|
if (falhas >= LimiteFalhasAntesReinit) {
|
|
AtualizarEstadoIniciado(false);
|
|
MostrarLog("Sensor marcado para reinicializacao controlada");
|
|
}
|
|
}
|
|
|
|
void RegistrarSucessoAquisicao() {
|
|
portENTER_CRITICAL(&_snapshotLock);
|
|
_falhasConsecutivas = 0;
|
|
portEXIT_CRITICAL(&_snapshotLock);
|
|
}
|
|
|
|
bool AmostraValida(
|
|
float tensaoBarramento,
|
|
float corrente,
|
|
float potencia,
|
|
float temperaturaDie
|
|
) {
|
|
if (!isfinite(tensaoBarramento) || !isfinite(corrente)) return false;
|
|
|
|
const float limiteTensao = isINA219 ? 32.5f : 85.0f;
|
|
if (tensaoBarramento < -0.5f || tensaoBarramento > limiteTensao) return false;
|
|
|
|
float limiteCorrenteA = 10.0f;
|
|
if (isINA219) {
|
|
limiteCorrenteA = 3.2f;
|
|
}
|
|
else if (_A_Shunt == TiposShuntCorrente::INA228_50A) {
|
|
limiteCorrenteA = 50.0f;
|
|
}
|
|
else if (_A_Shunt == TiposShuntCorrente::INA228_100A) {
|
|
limiteCorrenteA = 100.0f;
|
|
}
|
|
|
|
// A biblioteca Adafruit retorna corrente em mA.
|
|
if (fabsf(corrente) > (limiteCorrenteA * 1250.0f)) return false;
|
|
|
|
if (isINA219) {
|
|
return isfinite(potencia) && fabsf(potencia) <= 120000.0f;
|
|
}
|
|
|
|
return isfinite(temperaturaDie) &&
|
|
temperaturaDie >= -40.0f && temperaturaDie <= 150.0f;
|
|
}
|
|
|
|
void MostrarLog(String mensagem) {
|
|
if (DebugMode) {
|
|
PrintTela("[COR " + ID + "] " + mensagem);
|
|
}
|
|
}
|
|
|
|
void ConfigurarShuntINA228() {
|
|
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);
|
|
}
|
|
}
|
|
|
|
void AferirCorrente() {
|
|
if (!EstaIniciado()) return;
|
|
if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) {
|
|
// Contencao de mutex ou recovery em andamento nao prova falha do INA.
|
|
// Apenas pulamos esta amostra e tentamos novamente no proximo tick.
|
|
return;
|
|
}
|
|
|
|
// As APIs Adafruit abaixo nao propagam de forma util os erros de Wire.
|
|
// Este ping confirma o dispositivo no ramal antes da aquisicao.
|
|
if (!I2CService::VerificaEnderecoBarramento(_Endereco)) {
|
|
I2CService::LiberarAcessoI2C(ID_Num);
|
|
RegistrarFalhaAquisicao("dispositivo nao respondeu no endereco I2C");
|
|
return;
|
|
}
|
|
|
|
float novoBusVoltage = 0.0f;
|
|
float leituraCorrenteBruta = 0.0f;
|
|
float novaPotencia = 0.0f;
|
|
float novaTemperatura = 0.0f;
|
|
|
|
if (isINA219) {
|
|
novoBusVoltage = sINA219.getBusVoltage_V();
|
|
leituraCorrenteBruta = sINA219.getCurrent_mA();
|
|
novaPotencia = sINA219.getPower_mW();
|
|
}
|
|
else {
|
|
novoBusVoltage = sINA228.getBusVoltage_V();
|
|
leituraCorrenteBruta = sINA228.getCurrent_mA();
|
|
//power = sINA228.getPower_mW();
|
|
novaTemperatura = sINA228.readDieTemp();
|
|
//energy = sINA228.readEnergy();
|
|
//charge = sINA228.readCharge();
|
|
//codAlarme = (uint16_t)LerRegistradorINA(0x0B, 2);
|
|
}
|
|
|
|
const bool amostraValida = AmostraValida(
|
|
novoBusVoltage,
|
|
leituraCorrenteBruta,
|
|
novaPotencia,
|
|
novaTemperatura
|
|
);
|
|
|
|
if (!amostraValida) {
|
|
// Ainda dentro do ownership do I2C: o service consegue associar a
|
|
// falha ao ID/canal correto antes de o mutex ser liberado.
|
|
I2CService::NotificarFalhaDispositivo(
|
|
ID_Num,
|
|
"amostra INA invalida"
|
|
);
|
|
I2CService::LiberarAcessoI2C(ID_Num);
|
|
RegistrarFalhaAquisicao("amostra invalida/fora da faixa");
|
|
return;
|
|
}
|
|
|
|
// Somente amostras validadas entram no estado interno do filtro.
|
|
const float novaCorrente = filtro.updateEstimate(leituraCorrenteBruta);
|
|
|
|
I2CService::LiberarAcessoI2C(ID_Num);
|
|
RegistrarSucessoAquisicao();
|
|
|
|
// Publica o conjunto inteiro de uma vez. O CAN nunca observa metade da
|
|
// amostra anterior com metade da nova.
|
|
portENTER_CRITICAL(&_snapshotLock);
|
|
busVoltage = novoBusVoltage;
|
|
current = novaCorrente;
|
|
if (isINA219) {
|
|
power = novaPotencia;
|
|
}
|
|
else {
|
|
temperature = novaTemperatura;
|
|
}
|
|
portEXIT_CRITICAL(&_snapshotLock);
|
|
}
|
|
|
|
uint32_t LerRegistradorINA(uint8_t reg, uint8_t bytes) {
|
|
uint8_t buffer[4] = {0};
|
|
if (bytes > 4) return 0;
|
|
|
|
if (!I2CService::LeituraSegura(_Endereco, buffer, bytes, reg)) {
|
|
MostrarLog("Falha na leitura do registrador 0x" + String(reg, HEX));
|
|
return 0;
|
|
}
|
|
|
|
uint32_t valor = 0;
|
|
for (uint8_t i = 0; i < bytes; i++) {
|
|
valor = (valor << 8) | buffer[i];
|
|
}
|
|
|
|
return valor;
|
|
}
|
|
};
|
|
|
|
#endif
|