136 lines
3.0 KiB
C++
136 lines
3.0 KiB
C++
#include "SerialService.h"
|
|
#include "I2CService.h"
|
|
#include "Utils.h"
|
|
#include "IComponenteCAN.h"
|
|
#include <Wire.h>
|
|
|
|
#ifndef SensorMagneticoModel_v2
|
|
#define SensorMagneticoModel_v2
|
|
|
|
class SensorMagnetico_v2 : public ComponenteCAN {
|
|
public:
|
|
String ID;
|
|
uint8_t ID_Num = 0;
|
|
uint8_t _Endereco = 0x36;
|
|
int _CanalMUX = -1;
|
|
|
|
bool Iniciado = false;
|
|
uint16_t raw = 0;
|
|
float angle = 0;
|
|
|
|
SensorMagnetico_v2() {}
|
|
|
|
SensorMagnetico_v2(
|
|
const String& id,
|
|
uint8_t idNum,
|
|
uint8_t endereco = 0x36,
|
|
int canalMux = -1
|
|
) :
|
|
ID(id),
|
|
ID_Num(idNum),
|
|
_Endereco(endereco),
|
|
_CanalMUX(canalMux)
|
|
{
|
|
}
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
MostrarLog("Sensor ja inicializado");
|
|
return;
|
|
}
|
|
|
|
if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) {
|
|
MostrarLog("Falha ao solicitar acesso I2C");
|
|
return;
|
|
}
|
|
|
|
bool encontrado = I2CService::VerificaEnderecoBarramento(_Endereco);
|
|
Iniciado = encontrado;
|
|
|
|
if (!Iniciado) {
|
|
MostrarLog("Sensor nao iniciado, endereco " + String(_Endereco) + " nao encontrado");
|
|
} else {
|
|
MostrarLog("Sensor iniciado");
|
|
}
|
|
|
|
I2CService::LiberarAcessoI2C(ID_Num);
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado) {
|
|
MostrarLog("Sensor nao esta inicializado");
|
|
return;
|
|
}
|
|
|
|
Iniciado = false;
|
|
MostrarLog("Sensor desligado");
|
|
}
|
|
|
|
void RequisitarDados() {
|
|
if (!Iniciado) return;
|
|
AferirAngulo();
|
|
}
|
|
|
|
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 ang100 = (uint16_t)round(angle * 100.0f);
|
|
data.push_back(raw >> 8);
|
|
data.push_back(raw & 0xFF);
|
|
data.push_back(ang100 >> 8);
|
|
data.push_back(ang100 & 0xFF);
|
|
break;
|
|
}
|
|
}
|
|
|
|
return data;
|
|
}
|
|
|
|
private:
|
|
bool DebugMode = false;
|
|
static const uint8_t REG_RAW_ANGLE = 0x0C;
|
|
|
|
void MostrarLog(String mensagem) {
|
|
if (DebugMode) {
|
|
PrintTela("[MAG " + ID + "] " + mensagem);
|
|
}
|
|
}
|
|
|
|
void AferirAngulo() {
|
|
if (!Iniciado) return;
|
|
if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) return;
|
|
|
|
raw = LerRawAngle();
|
|
angle = RawToDegrees(raw);
|
|
|
|
I2CService::LiberarAcessoI2C(ID_Num);
|
|
}
|
|
|
|
uint16_t LerRawAngle() {
|
|
uint8_t buffer[2] = {0};
|
|
|
|
if (!I2CService::LeituraSegura(_Endereco, buffer, 2, REG_RAW_ANGLE)) {
|
|
MostrarLog("Falha na leitura do RAW_ANGLE");
|
|
return 0;
|
|
}
|
|
|
|
uint16_t valor = ((uint16_t)buffer[0] << 8) | buffer[1];
|
|
valor &= 0x0FFF;
|
|
return valor;
|
|
}
|
|
|
|
float RawToDegrees(uint16_t valorRaw) {
|
|
return (valorRaw * 360.0f) / 4096.0f;
|
|
}
|
|
};
|
|
|
|
#endif |