543 lines
11 KiB
C++
543 lines
11 KiB
C++
#include "SerialService.h"
|
|
#include "Pinout.h"
|
|
#include "Utils.h"
|
|
#include "IComponenteCAN.h"
|
|
|
|
#ifndef SensorPressaoModel_v2_h
|
|
#define SensorPressaoModel_v2_h
|
|
|
|
class SensorPressao_v2 : public ComponenteCAN {
|
|
public:
|
|
SensorPressao_v2(
|
|
const String& id,
|
|
uint8_t idNum,
|
|
int pino,
|
|
TiposBarramentos barramento = TiposBarramentos::CONTROLADOR,
|
|
double vmin = 0.0,
|
|
double vmax = 4.5,
|
|
double pmax = 174.045
|
|
)
|
|
: _ID(id),
|
|
ID_Num(idNum),
|
|
_pino(
|
|
pino,
|
|
barramento,
|
|
TiposPinos::_INPUT,
|
|
TiposLeituras::ANALOGICO
|
|
),
|
|
_Vmin(vmin),
|
|
_Vmax(vmax),
|
|
_Pmax(pmax) {
|
|
}
|
|
|
|
String _ID;
|
|
uint8_t ID_Num = 0;
|
|
PinoModel _pino;
|
|
|
|
double _Vmin = 0.0;
|
|
double _Vmax = 4.5;
|
|
double _Pmax = 174.045;
|
|
|
|
double GanhoCalibracaoPressao = 1.1361506;
|
|
double OffsetCalibracaoPressao = -14.887795;
|
|
|
|
int leitura = 0;
|
|
double Tensao = 0.0;
|
|
|
|
/*
|
|
* Pressão convertida e calibrada, mas sem filtro.
|
|
* Será usada futuramente para proteção rápida.
|
|
*/
|
|
double PressaoBruta = 0.0;
|
|
|
|
/*
|
|
* Pressão filtrada usada pela malha de controle.
|
|
*/
|
|
double Pressao = 0.0;
|
|
|
|
bool LeituraValida = false;
|
|
bool FiltroInicializado = false;
|
|
bool Iniciado = false;
|
|
|
|
unsigned long UltimaLeituraMs = 0;
|
|
unsigned long UltimaTentativaLeituraMs = 0;
|
|
unsigned long UltimaAtualizacaoFiltroMs = 0;
|
|
|
|
unsigned int FalhasConsecutivas = 0;
|
|
|
|
/*
|
|
* Evita aquisições duplicadas muito próximas,
|
|
* caso controle e telemetria solicitem leitura.
|
|
*/
|
|
unsigned long IntervaloMinimoLeituraMs = 5;
|
|
|
|
/*
|
|
* Constante de tempo do filtro.
|
|
*
|
|
* 0,10 s:
|
|
* - responde rápido;
|
|
* - reduz ruído do ADC;
|
|
* - não adiciona atraso excessivo ao PID.
|
|
*/
|
|
double TauFiltroPressaoSeg = 0.10;
|
|
|
|
/*
|
|
* Faixa elétrica plausível.
|
|
*
|
|
* O sensor calibrado em 0 psi deve ficar acima
|
|
* do limite mínimo.
|
|
*/
|
|
double TensaoMinimaValida = 0.10;
|
|
double TensaoMaximaValida = 3.25;
|
|
|
|
portMUX_TYPE _stateMux =
|
|
portMUX_INITIALIZER_UNLOCKED;
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
MostrarLog("Sensor ja inicializado");
|
|
return;
|
|
}
|
|
|
|
_pino.Conectar();
|
|
Iniciado = _pino.Definido();
|
|
|
|
ResetarEstado();
|
|
|
|
if (Iniciado) {
|
|
MostrarLog("Sensor iniciado");
|
|
} else {
|
|
MostrarLog("Sensor nao iniciado");
|
|
}
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado) {
|
|
MostrarLog("Sensor nao esta inicializado");
|
|
return;
|
|
}
|
|
|
|
Iniciado = false;
|
|
_pino.Desconectar();
|
|
|
|
ResetarEstado();
|
|
|
|
MostrarLog("Sensor desligado");
|
|
}
|
|
|
|
void RequisitarDados() {
|
|
AtualizarLeitura();
|
|
}
|
|
|
|
void AtualizarLeitura() {
|
|
if (!Iniciado)
|
|
return;
|
|
|
|
unsigned long agora = millis();
|
|
|
|
/*
|
|
* Impede duas aquisições simultâneas e evita
|
|
* atualizações duplicadas muito próximas.
|
|
*/
|
|
portENTER_CRITICAL(&_stateMux);
|
|
|
|
bool leituraMuitoRecente =
|
|
UltimaTentativaLeituraMs > 0 &&
|
|
(agora - UltimaTentativaLeituraMs) <
|
|
IntervaloMinimoLeituraMs;
|
|
|
|
if (_leituraEmAndamento || leituraMuitoRecente) {
|
|
portEXIT_CRITICAL(&_stateMux);
|
|
return;
|
|
}
|
|
|
|
_leituraEmAndamento = true;
|
|
UltimaTentativaLeituraMs = agora;
|
|
|
|
portEXIT_CRITICAL(&_stateMux);
|
|
|
|
/*
|
|
* Aquisição física fora da região crítica.
|
|
*/
|
|
int novaLeitura = _pino.get();
|
|
|
|
double novaTensao =
|
|
fmap(
|
|
(double)novaLeitura,
|
|
0.0,
|
|
4095.0,
|
|
0.0,
|
|
3300.0
|
|
) / 1000.0;
|
|
|
|
bool configuracaoValida =
|
|
isfinite(_Vmin) &&
|
|
isfinite(_Vmax) &&
|
|
isfinite(_Pmax) &&
|
|
isfinite(GanhoCalibracaoPressao) &&
|
|
isfinite(OffsetCalibracaoPressao) &&
|
|
_Vmax > _Vmin &&
|
|
_Pmax > 0.0;
|
|
|
|
bool leituraEletricaValida =
|
|
novaLeitura > 0 &&
|
|
novaLeitura < 4095 &&
|
|
isfinite(novaTensao) &&
|
|
novaTensao >= TensaoMinimaValida &&
|
|
novaTensao <= TensaoMaximaValida;
|
|
|
|
if (
|
|
!configuracaoValida ||
|
|
!leituraEletricaValida
|
|
) {
|
|
RegistrarLeituraInvalida(
|
|
novaLeitura,
|
|
novaTensao
|
|
);
|
|
|
|
return;
|
|
}
|
|
|
|
double pressaoCalculada =
|
|
fmap(
|
|
novaTensao,
|
|
_Vmin,
|
|
_Vmax,
|
|
0.0,
|
|
_Pmax
|
|
);
|
|
|
|
pressaoCalculada =
|
|
(
|
|
pressaoCalculada *
|
|
GanhoCalibracaoPressao
|
|
) +
|
|
OffsetCalibracaoPressao;
|
|
|
|
if (!isfinite(pressaoCalculada)) {
|
|
RegistrarLeituraInvalida(
|
|
novaLeitura,
|
|
novaTensao
|
|
);
|
|
|
|
return;
|
|
}
|
|
|
|
double novaPressaoBruta =
|
|
constrain(
|
|
pressaoCalculada,
|
|
0.0,
|
|
_Pmax
|
|
);
|
|
|
|
double pressaoAnterior;
|
|
unsigned long ultimaAtualizacao;
|
|
bool filtroInicializadoAnterior;
|
|
|
|
portENTER_CRITICAL(&_stateMux);
|
|
|
|
pressaoAnterior = Pressao;
|
|
ultimaAtualizacao =
|
|
UltimaAtualizacaoFiltroMs;
|
|
|
|
filtroInicializadoAnterior =
|
|
FiltroInicializado;
|
|
|
|
portEXIT_CRITICAL(&_stateMux);
|
|
|
|
double novaPressaoFiltrada;
|
|
|
|
if (
|
|
!filtroInicializadoAnterior ||
|
|
ultimaAtualizacao == 0
|
|
) {
|
|
/*
|
|
* A primeira leitura começa no valor real.
|
|
* O filtro não parte artificialmente de zero.
|
|
*/
|
|
novaPressaoFiltrada =
|
|
novaPressaoBruta;
|
|
} else {
|
|
double dt =
|
|
(agora - ultimaAtualizacao) /
|
|
1000.0;
|
|
|
|
dt = constrain(
|
|
dt,
|
|
0.001,
|
|
0.500
|
|
);
|
|
|
|
double tau =
|
|
TauFiltroPressaoSeg;
|
|
|
|
if (
|
|
!isfinite(tau) ||
|
|
tau < 0.001
|
|
) {
|
|
tau = 0.001;
|
|
}
|
|
|
|
double alpha =
|
|
dt / (tau + dt);
|
|
|
|
alpha = constrain(
|
|
alpha,
|
|
0.01,
|
|
1.0
|
|
);
|
|
|
|
novaPressaoFiltrada =
|
|
(alpha * novaPressaoBruta) +
|
|
(
|
|
(1.0 - alpha) *
|
|
pressaoAnterior
|
|
);
|
|
}
|
|
|
|
/*
|
|
* Publica a nova amostra de forma consistente.
|
|
*/
|
|
portENTER_CRITICAL(&_stateMux);
|
|
|
|
leitura = novaLeitura;
|
|
Tensao = novaTensao;
|
|
|
|
PressaoBruta = novaPressaoBruta;
|
|
Pressao = novaPressaoFiltrada;
|
|
|
|
LeituraValida = true;
|
|
FiltroInicializado = true;
|
|
|
|
UltimaLeituraMs = agora;
|
|
UltimaAtualizacaoFiltroMs = agora;
|
|
|
|
FalhasConsecutivas = 0;
|
|
_leituraEmAndamento = false;
|
|
|
|
portEXIT_CRITICAL(&_stateMux);
|
|
}
|
|
|
|
unsigned long IdadeLeituraMs() const {
|
|
unsigned long ultimaLeitura =
|
|
UltimaLeituraMs;
|
|
|
|
if (ultimaLeitura == 0)
|
|
return ULONG_MAX;
|
|
|
|
return millis() - ultimaLeitura;
|
|
}
|
|
|
|
bool LeituraRecente(
|
|
unsigned long timeoutMs = 250
|
|
) const {
|
|
return
|
|
LeituraValida &&
|
|
UltimaLeituraMs > 0 &&
|
|
IdadeLeituraMs() <= timeoutMs;
|
|
}
|
|
|
|
/*
|
|
* Snapshot consistente para a bomba.
|
|
*/
|
|
void ObterSnapshotControle(
|
|
double& pressaoBruta,
|
|
double& pressaoFiltrada,
|
|
bool& leituraValida,
|
|
unsigned long& idadeLeituraMs
|
|
) {
|
|
unsigned long ultimaLeitura;
|
|
|
|
portENTER_CRITICAL(&_stateMux);
|
|
|
|
pressaoBruta = PressaoBruta;
|
|
pressaoFiltrada = Pressao;
|
|
leituraValida = LeituraValida;
|
|
ultimaLeitura = UltimaLeituraMs;
|
|
|
|
portEXIT_CRITICAL(&_stateMux);
|
|
|
|
idadeLeituraMs =
|
|
ultimaLeitura == 0
|
|
? ULONG_MAX
|
|
: millis() - ultimaLeitura;
|
|
|
|
leituraValida =
|
|
leituraValida &&
|
|
ultimaLeitura > 0;
|
|
}
|
|
|
|
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: {
|
|
int leituraAtual;
|
|
double pressaoAtual;
|
|
double tensaoAtual;
|
|
|
|
portENTER_CRITICAL(&_stateMux);
|
|
|
|
leituraAtual = leitura;
|
|
pressaoAtual = Pressao;
|
|
tensaoAtual = Tensao;
|
|
|
|
portEXIT_CRITICAL(&_stateMux);
|
|
|
|
double pressaoEscaladaDouble =
|
|
round(
|
|
pressaoAtual * 100.0
|
|
);
|
|
|
|
pressaoEscaladaDouble =
|
|
constrain(
|
|
pressaoEscaladaDouble,
|
|
0.0,
|
|
65535.0
|
|
);
|
|
|
|
double tensaoEscaladaDouble =
|
|
round(
|
|
tensaoAtual * 1000.0
|
|
);
|
|
|
|
tensaoEscaladaDouble =
|
|
constrain(
|
|
tensaoEscaladaDouble,
|
|
0.0,
|
|
65535.0
|
|
);
|
|
|
|
uint16_t pressaoEscalada =
|
|
(uint16_t)pressaoEscaladaDouble;
|
|
|
|
uint16_t tensaoEscalada =
|
|
(uint16_t)tensaoEscaladaDouble;
|
|
|
|
uint16_t leituraEscalada =
|
|
leituraAtual < 0
|
|
? 0
|
|
: (
|
|
leituraAtual > 65535
|
|
? 65535
|
|
: (uint16_t)leituraAtual
|
|
);
|
|
|
|
data.push_back(
|
|
(uint8_t)(leituraEscalada >> 8)
|
|
);
|
|
|
|
data.push_back(
|
|
(uint8_t)(leituraEscalada & 0xFF)
|
|
);
|
|
|
|
data.push_back(
|
|
(uint8_t)(pressaoEscalada >> 8)
|
|
);
|
|
|
|
data.push_back(
|
|
(uint8_t)(pressaoEscalada & 0xFF)
|
|
);
|
|
|
|
data.push_back(
|
|
(uint8_t)(tensaoEscalada >> 8)
|
|
);
|
|
|
|
data.push_back(
|
|
(uint8_t)(tensaoEscalada & 0xFF)
|
|
);
|
|
|
|
break;
|
|
}
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return data;
|
|
}
|
|
|
|
private:
|
|
bool DebugMode = false;
|
|
bool _leituraEmAndamento = false;
|
|
|
|
void RegistrarLeituraInvalida(
|
|
int novaLeitura,
|
|
double novaTensao
|
|
) {
|
|
portENTER_CRITICAL(&_stateMux);
|
|
|
|
/*
|
|
* Atualizamos ADC e tensão para diagnóstico,
|
|
* mas preservamos a última pressão válida.
|
|
*/
|
|
leitura = novaLeitura;
|
|
Tensao =
|
|
isfinite(novaTensao)
|
|
? novaTensao
|
|
: 0.0;
|
|
|
|
LeituraValida = false;
|
|
|
|
if (FalhasConsecutivas < UINT_MAX) {
|
|
FalhasConsecutivas++;
|
|
}
|
|
|
|
_leituraEmAndamento = false;
|
|
|
|
portEXIT_CRITICAL(&_stateMux);
|
|
}
|
|
|
|
void ResetarEstado() {
|
|
portENTER_CRITICAL(&_stateMux);
|
|
|
|
leitura = 0;
|
|
Tensao = 0.0;
|
|
|
|
PressaoBruta = 0.0;
|
|
Pressao = 0.0;
|
|
|
|
LeituraValida = false;
|
|
FiltroInicializado = false;
|
|
|
|
UltimaLeituraMs = 0;
|
|
UltimaTentativaLeituraMs = 0;
|
|
UltimaAtualizacaoFiltroMs = 0;
|
|
|
|
FalhasConsecutivas = 0;
|
|
_leituraEmAndamento = false;
|
|
|
|
portEXIT_CRITICAL(&_stateMux);
|
|
}
|
|
|
|
void MostrarLog(
|
|
String mensagem
|
|
) {
|
|
if (DebugMode) {
|
|
PrintTela(
|
|
"[PRS " +
|
|
_ID +
|
|
"] " +
|
|
mensagem
|
|
);
|
|
}
|
|
}
|
|
};
|
|
|
|
#endif |