154 lines
3.9 KiB
C++
154 lines
3.9 KiB
C++
#include "SerialService.h"
|
|
#include "Pinout.h"
|
|
#include "Utils.h"
|
|
#include <SimpleKalmanFilter.h>
|
|
|
|
#ifndef SensorPressaoModel
|
|
#define SensorPressaoModel
|
|
|
|
class SensorPressao {
|
|
public:
|
|
|
|
static std::vector<uint8_t> ConfigurarSensor(std::vector<SensorPressao*>& lista, std::vector<uint8_t>& data, const String& Mod_ID);
|
|
|
|
SensorPressao(String _modID, String _id) {
|
|
Mod_ID = _modID;
|
|
_ID = _id;
|
|
}
|
|
|
|
// Definições
|
|
String Mod_ID = "";
|
|
String _ID;
|
|
int ID_Num;
|
|
PinoModel _pino;
|
|
|
|
double _Vmin = 0.0;
|
|
double _Vmax = 4.5;
|
|
double _Pmax = 174.045;
|
|
|
|
double leitura = 0;
|
|
double Pressao = 0;
|
|
|
|
SimpleKalmanFilter filtro = SimpleKalmanFilter(2, 2, 0.01);
|
|
|
|
bool Iniciado = false;
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
PrintTela(_ID + " ja inicializado");
|
|
return;
|
|
}
|
|
|
|
leitura = 0;
|
|
|
|
_pino.Conectar(); // Conecta o pino antes de criar a tarefa
|
|
|
|
Iniciado = _pino.Definido();
|
|
|
|
if (Iniciado) {
|
|
PrintTela(_ID + " iniciado");
|
|
}
|
|
else {
|
|
PrintTela(_ID + " nao iniciado");
|
|
}
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado) {
|
|
PrintTela(_ID + " nao esta inicializado");
|
|
return;
|
|
}
|
|
|
|
_pino.Desconectar(); // Desconecta o pino
|
|
|
|
Iniciado = false;
|
|
|
|
PrintTela(_ID + " Desligado");
|
|
}
|
|
|
|
void RequisitarDados() {
|
|
AferirPressao();
|
|
}
|
|
|
|
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);
|
|
data.push_back(pressaoEscalada >> 8);
|
|
data.push_back(pressaoEscalada & 0xFF);
|
|
break;
|
|
}
|
|
}
|
|
return data;
|
|
}
|
|
|
|
|
|
private:
|
|
void AferirPressao() {
|
|
if (Iniciado) {
|
|
leitura = _pino.get();
|
|
double leituraPressao = filtro.updateEstimate(leitura);
|
|
|
|
double tensao = fmap(leituraPressao, 0.0, 4095.0, 0.0, 3300.0) / 1000.0;
|
|
Pressao = fmap(tensao, _Vmin, _Vmax, 0.0, _Pmax);
|
|
}
|
|
}
|
|
|
|
|
|
};
|
|
|
|
|
|
std::vector<uint8_t> SensorPressao::ConfigurarSensor(std::vector<SensorPressao*>& 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](SensorPressao* s) { return s->ID_Num == idNum; });
|
|
bool jaExiste = it != lista.end();
|
|
SensorPressao* sensor;
|
|
switch (posicao) {
|
|
case CanMessagePosicaoDados::Config1: {
|
|
if (data.size() < 5) return status;
|
|
bool conectar = data[3] == 1;
|
|
int pino = data[4];
|
|
if (conectar) {
|
|
if (!jaExiste) {
|
|
sensor = new SensorPressao(Mod_ID, "sPRS_" + String(idNum));
|
|
}
|
|
else {
|
|
sensor = *it;
|
|
}
|
|
if (!sensor->Iniciado) {
|
|
sensor->ID_Num = idNum;
|
|
sensor->_pino = PinoModel(pino, CONTROLADOR, _INPUT, ANALOGICO);
|
|
sensor->Inicializar();
|
|
if (!jaExiste) {
|
|
lista.push_back(sensor);
|
|
PrintTela("Sensor de Pressao adicionado via CAN: sPRS_" + String(idNum));
|
|
}
|
|
}
|
|
status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
|
|
} else {
|
|
if (jaExiste) {
|
|
sensor = *it;
|
|
sensor->Desligar();
|
|
status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
|
|
delete sensor;
|
|
lista.erase(it);
|
|
PrintTela("Sensor de Pressao removido via CAN: sPRS_" + String(idNum));
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
return status;
|
|
}
|
|
|
|
#endif |