347 lines
9.8 KiB
C++
347 lines
9.8 KiB
C++
#ifndef SensorIMUModel
|
|
#define SensorIMUModel
|
|
|
|
#include "SerialService.h"
|
|
#include "I2CService.h"
|
|
#include <MPU9250_asukiaaa.h>
|
|
#include <Adafruit_BMP280.h>
|
|
#include <MahonyAHRS.h>
|
|
#include <Wire.h>
|
|
|
|
class SensorIMU {
|
|
public:
|
|
|
|
String Mod_ID;
|
|
String _ID;
|
|
int ID_Num;
|
|
bool Iniciado = false;
|
|
bool MpuIniciado = false;
|
|
bool BmpIniciado = false;
|
|
|
|
// Endereços I2C
|
|
byte _EnderecoMPU; // 0x68
|
|
byte _EnderecoBMP; // 0x76
|
|
|
|
// MPU9250 (IMU 9 eixos)
|
|
MPU9250_asukiaaa mpu;
|
|
|
|
// BMP280 (Temperatura, Pressão, Altitude)
|
|
Adafruit_BMP280 bmp;
|
|
|
|
Mahony filter;
|
|
int filterHz = 100;
|
|
TickType_t filterDelay = pdMS_TO_TICKS(roundf(1000.0f / filterHz));
|
|
|
|
// Leituras principais
|
|
float AccX = 0, AccY = 0, AccZ = 0;
|
|
float GyroX = 0, GyroY = 0, GyroZ = 0;
|
|
float MagX = 0, MagY = 0, MagZ = 0;
|
|
float Temp = 0;
|
|
float Pressao = 0;
|
|
float Altitude = 0;
|
|
float Roll = 0;
|
|
float Pitch = 0;
|
|
float Yaw = 0;
|
|
|
|
SensorIMU(String _modID, String _id) {
|
|
Mod_ID = _modID;
|
|
_ID = _id;
|
|
}
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
MostrarLog("Sensor ja inicializado");
|
|
return;
|
|
}
|
|
|
|
if (!I2CService::SolicitarAcessoI2C(ID_Num)) return;
|
|
|
|
MpuIniciado = I2CService::VerificaEnderecoBarramento(_EnderecoMPU);
|
|
if (MpuIniciado) {
|
|
mpu.setWire(&Wire);
|
|
mpu.beginAccel();
|
|
mpu.beginGyro();
|
|
//mpu.beginMag();
|
|
|
|
filterDelay = 1000.0f / filterHz;
|
|
xTaskCreatePinnedToCore(&SensorIMU::IMUTaskWrapper, "IMUTask", 4096, this, 10, &IMUTaskHandle, tskNO_AFFINITY);
|
|
|
|
MostrarLog("MPU9250 iniciado");
|
|
} else {
|
|
MostrarLog("MPU9250 nao encontrado no endereco " + String(_EnderecoMPU));
|
|
}
|
|
|
|
bool BmpEncontrado = I2CService::VerificaEnderecoBarramento(_EnderecoBMP);
|
|
if (BmpEncontrado) {
|
|
BmpIniciado = bmp.begin(_EnderecoBMP);
|
|
if (BmpIniciado) {
|
|
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
|
|
Adafruit_BMP280::SAMPLING_X2,
|
|
Adafruit_BMP280::SAMPLING_X16,
|
|
Adafruit_BMP280::FILTER_X16,
|
|
Adafruit_BMP280::STANDBY_MS_500);
|
|
MostrarLog("BMP280 iniciado");
|
|
}
|
|
else {
|
|
MostrarLog("Erro ao iniciar BMP280");
|
|
}
|
|
}
|
|
else {
|
|
MostrarLog("BMP280 nao encontrado no endereco " + String(_EnderecoBMP));
|
|
}
|
|
|
|
Iniciado = MpuIniciado || BmpIniciado;
|
|
|
|
if (Iniciado) {
|
|
MostrarLog("Sensor iniciado");
|
|
}
|
|
|
|
I2CService::LiberarAcessoI2C(ID_Num);
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado) {
|
|
MostrarLog("Sensor nao esta inicializado");
|
|
return;
|
|
}
|
|
|
|
Iniciado = false;
|
|
MpuIniciado = false;
|
|
BmpIniciado = false;
|
|
|
|
if (I2CService::QuemEstaUsando() == ID_Num) {
|
|
while (I2CService::QuemEstaUsando() == ID_Num) {
|
|
vTaskDelay(10);
|
|
}
|
|
}
|
|
|
|
// Parar a execução das tarefas
|
|
if (IMUTaskHandle != NULL) {
|
|
vTaskDelete(IMUTaskHandle);
|
|
IMUTaskHandle = NULL;
|
|
}
|
|
|
|
MostrarLog("Sensor Desligado");
|
|
}
|
|
|
|
void RequisitarDados() {
|
|
AferirDadosIMU();
|
|
AferirDadosTMP();
|
|
}
|
|
|
|
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: { // Roll, Pitch, Yaw
|
|
int16_t roll = Roll * 100;
|
|
int16_t pitch = Pitch * 100;
|
|
int16_t yaw = Yaw * 100;
|
|
|
|
data.push_back(roll >> 8); data.push_back(roll & 0xFF);
|
|
data.push_back(pitch >> 8); data.push_back(pitch & 0xFF);
|
|
data.push_back(yaw >> 8); data.push_back(yaw & 0xFF);
|
|
break;
|
|
}
|
|
|
|
case CanMessagePosicaoDados::Dados2: { // Temp, Pressão, Altitude
|
|
int16_t temp = Temp * 100;
|
|
uint16_t pressao = Pressao / 10; // Ex: 100000 Pa → 10000 (precisão: 10 Pa)
|
|
int16_t altitude = Altitude / 10;
|
|
|
|
data.push_back(temp >> 8); data.push_back(temp & 0xFF);
|
|
data.push_back(pressao >> 8); data.push_back(pressao & 0xFF);
|
|
data.push_back(altitude >> 8); data.push_back(altitude & 0xFF);
|
|
break;
|
|
}
|
|
|
|
case CanMessagePosicaoDados::Dados3: { // Acc
|
|
int16_t accX = AccX * 100;
|
|
int16_t accY = AccY * 100;
|
|
int16_t accZ = AccZ * 100;
|
|
|
|
data.push_back(accX >> 8); data.push_back(accX & 0xFF);
|
|
data.push_back(accY >> 8); data.push_back(accY & 0xFF);
|
|
data.push_back(accZ >> 8); data.push_back(accZ & 0xFF);
|
|
break;
|
|
}
|
|
|
|
case CanMessagePosicaoDados::Dados4: { // Gyro
|
|
int16_t gyroX = GyroX * 100;
|
|
int16_t gyroY = GyroY * 100;
|
|
int16_t gyroZ = GyroZ * 100;
|
|
|
|
data.push_back(gyroX >> 8); data.push_back(gyroX & 0xFF);
|
|
data.push_back(gyroY >> 8); data.push_back(gyroY & 0xFF);
|
|
data.push_back(gyroZ >> 8); data.push_back(gyroZ & 0xFF);
|
|
break;
|
|
}
|
|
|
|
case CanMessagePosicaoDados::Dados5: { // Mag
|
|
int16_t magX = MagX * 100;
|
|
int16_t magY = MagY * 100;
|
|
int16_t magZ = MagZ * 100;
|
|
|
|
data.push_back(magX >> 8); data.push_back(magX & 0xFF);
|
|
data.push_back(magY >> 8); data.push_back(magY & 0xFF);
|
|
data.push_back(magZ >> 8); data.push_back(magZ & 0xFF);
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
return data;
|
|
}
|
|
|
|
static std::vector<uint8_t> ConfigurarSensor(std::vector<SensorIMU*>& 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](SensorIMU* s) { return s->ID_Num == idNum; });
|
|
bool jaExiste = it != lista.end();
|
|
SensorIMU* sensor;
|
|
switch (posicao) {
|
|
case CanMessagePosicaoDados::Config1: {
|
|
if (data.size() < 6) return status;
|
|
bool conectar = data[3] == 1;
|
|
uint8_t enderecoMpu = data[4];
|
|
uint8_t enderecoBmp = data[5];
|
|
if (conectar) {
|
|
if (!jaExiste) {
|
|
sensor = new SensorIMU(Mod_ID, "sIMU_" + String(idNum));
|
|
}
|
|
else {
|
|
sensor = *it;
|
|
}
|
|
if (!sensor->Iniciado) {
|
|
sensor->ID_Num = idNum;
|
|
sensor->_EnderecoMPU = enderecoMpu;
|
|
sensor->_EnderecoBMP = enderecoBmp;
|
|
sensor->Inicializar();
|
|
if (!jaExiste) {
|
|
lista.push_back(sensor);
|
|
sensor->MostrarLog("Sensor IMU adicionado via CAN: sIMU_" + String(idNum));
|
|
}
|
|
}
|
|
status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
|
|
} else {
|
|
if (jaExiste) {
|
|
sensor = *it;
|
|
sensor->Desligar();
|
|
status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
|
|
sensor->MostrarLog("Sensor IMU removido via CAN: sIMU_" + String(idNum));
|
|
delete sensor;
|
|
lista.erase(it);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
return status;
|
|
}
|
|
|
|
|
|
private:
|
|
bool DebugMode = true;
|
|
|
|
void MostrarLog(String mensagem) {
|
|
if (DebugMode) {
|
|
PrintTela("[IMU " + _ID + "]" + mensagem);
|
|
}
|
|
}
|
|
|
|
TaskHandle_t IMUTaskHandle = NULL;
|
|
static void IMUTaskWrapper(void *pvParameters) {
|
|
SensorIMU *sensor = static_cast<SensorIMU*>(pvParameters);
|
|
sensor->IMUTask();
|
|
}
|
|
|
|
void IMUTask() {
|
|
TickType_t xLastWakeTime = xTaskGetTickCount();
|
|
|
|
while (1) {
|
|
if (MpuIniciado) {
|
|
AplicarFiltroMahony();
|
|
vTaskDelayUntil(&xLastWakeTime, filterDelay);
|
|
}
|
|
else {
|
|
vTaskDelay(1000);
|
|
}
|
|
}
|
|
}
|
|
|
|
void AplicarFiltroMahony() {
|
|
filter.updateIMU(GyroX, GyroY, GyroZ, AccX, AccY, AccZ);
|
|
|
|
// Recupera os quaternions
|
|
float q0 = filter.getQ0();
|
|
float q1 = filter.getQ1();
|
|
float q2 = filter.getQ2();
|
|
float q3 = filter.getQ3();
|
|
|
|
// Vetor "up"
|
|
float upX = 2 * (q1 * q3 - q0 * q2);
|
|
float upY = 2 * (q2 * q3 + q0 * q1);
|
|
float upZ = 1 - 2 * (q1 * q1 + q2 * q2);
|
|
|
|
// Calcula os ângulos corrigidos
|
|
Roll = atan2(upY, upZ) * RAD_TO_DEG;
|
|
Roll = (Roll > 0) ? Roll - 180 : Roll + 180;
|
|
Pitch = -(atan2(-upX, sqrt(upY * upY + upZ * upZ)) * RAD_TO_DEG);
|
|
Yaw = atan2(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3) * RAD_TO_DEG;
|
|
}
|
|
|
|
void AferirDadosIMU() {
|
|
if (!MpuIniciado) return;
|
|
|
|
if (!I2CService::SolicitarAcessoI2C(ID_Num)) return;
|
|
|
|
mpu.accelUpdate();
|
|
mpu.gyroUpdate();
|
|
mpu.magUpdate();
|
|
|
|
AccX = mpu.accelX();
|
|
AccY = mpu.accelY();
|
|
AccZ = mpu.accelZ();
|
|
|
|
GyroX = mpu.gyroX();
|
|
GyroY = mpu.gyroY();
|
|
GyroZ = mpu.gyroZ();
|
|
|
|
/*MagX = mpu.magX();
|
|
MagY = mpu.magY();
|
|
MagZ = mpu.magZ();*/
|
|
|
|
I2CService::LiberarAcessoI2C(ID_Num);
|
|
|
|
MostrarLog("AccX: " + String(AccX) + ", AccY: " + String(AccY) + ", AccZ: " + String(AccZ) + ", GyroX: " + String(GyroX) + ", GyroY: " + String(GyroY) + ", GyroZ: " + String(GyroZ));
|
|
}
|
|
|
|
void AferirDadosTMP() {
|
|
if (!BmpIniciado) return;
|
|
|
|
if (!I2CService::SolicitarAcessoI2C(ID_Num)) return;
|
|
|
|
Temp = bmp.readTemperature();
|
|
Pressao = bmp.readPressure();
|
|
Altitude = bmp.readAltitude();
|
|
|
|
I2CService::LiberarAcessoI2C(ID_Num);
|
|
|
|
MostrarLog("Temperatura: " + String(Temp) + ", Pressao: " + String(Pressao) + ", Altitude: " + String(Altitude));
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif
|