152 lines
3.4 KiB
C++
152 lines
3.4 KiB
C++
#include "SerialService.h"
|
|
#include "Pinout.h"
|
|
#include "Utils.h"
|
|
#include "IComponenteCAN.h"
|
|
#include <SimpleKalmanFilter.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),
|
|
filtro(2, 2, 0.01) {
|
|
}
|
|
|
|
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;
|
|
double Pressao = 0.0;
|
|
|
|
bool Iniciado = false;
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
MostrarLog("Sensor ja inicializado");
|
|
return;
|
|
}
|
|
|
|
leitura = 0;
|
|
Tensao = 0.0;
|
|
Pressao = 0.0;
|
|
|
|
_pino.Conectar();
|
|
Iniciado = _pino.Definido();
|
|
|
|
if (Iniciado) {
|
|
MostrarLog("Sensor iniciado");
|
|
} else {
|
|
MostrarLog("Sensor nao iniciado");
|
|
}
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado) {
|
|
MostrarLog("Sensor nao esta inicializado");
|
|
return;
|
|
}
|
|
|
|
_pino.Desconectar();
|
|
|
|
Iniciado = false;
|
|
leitura = 0;
|
|
Tensao = 0.0;
|
|
Pressao = 0.0;
|
|
|
|
MostrarLog("Sensor desligado");
|
|
}
|
|
|
|
void RequisitarDados() {
|
|
AtualizarLeitura();
|
|
|
|
//MostrarLog("Leitura: " + String(leitura) + ", Pressao: " + String(Pressao));
|
|
}
|
|
|
|
void AtualizarLeitura() {
|
|
if (!Iniciado) return;
|
|
|
|
leitura = _pino.get();
|
|
double leituraFiltrada = filtro.updateEstimate(leitura);
|
|
|
|
Tensao = fmap(leituraFiltrada, 0.0, 4095.0, 0.0, 3300.0) / 1000.0;
|
|
|
|
double pressaoBruta = fmap(Tensao, _Vmin, _Vmax, 0.0, _Pmax);
|
|
|
|
Pressao = (pressaoBruta * GanhoCalibracaoPressao) + OffsetCalibracaoPressao;
|
|
|
|
if (Pressao < 0) Pressao = 0;
|
|
if (Pressao > _Pmax) Pressao = _Pmax;
|
|
}
|
|
|
|
|
|
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 pressaoEscalada = (uint16_t)(Pressao * 100.0);
|
|
uint16_t tensaoEscalada = (uint16_t)(Tensao * 1000.0);
|
|
|
|
data.push_back((uint8_t)(leitura >> 8));
|
|
data.push_back((uint8_t)(leitura & 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;
|
|
SimpleKalmanFilter filtro;
|
|
|
|
void MostrarLog(String mensagem) {
|
|
if (DebugMode) {
|
|
PrintTela("[PRS " + _ID + "] " + mensagem);
|
|
}
|
|
}
|
|
};
|
|
|
|
#endif
|