137 lines
3.4 KiB
C++
137 lines
3.4 KiB
C++
#ifndef SensorIMUModel
|
|
#define SensorIMUModel
|
|
|
|
#include <MPU9250_WE.h>
|
|
#include <Adafruit_BMP280.h>
|
|
#include <Wire.h>
|
|
#include "SerialService.h"
|
|
#include "Pinout.h"
|
|
|
|
class SensorIMU {
|
|
public:
|
|
String Mod_ID;
|
|
String _ID;
|
|
int ID_Num;
|
|
bool Iniciado = false;
|
|
|
|
// Endereços I2C
|
|
#define MPU_ADDR 0x68
|
|
#define BMP_ADDR 0x76
|
|
|
|
// MPU9250 (IMU 9 eixos)
|
|
MPU9250_WE mpu = MPU9250_WE(MPU_ADDR);
|
|
|
|
// BMP280 (Temperatura, Pressão, Altitude)
|
|
Adafruit_BMP280 bmp;
|
|
|
|
// 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;
|
|
|
|
SensorIMU(String _modID, String _id) {
|
|
Mod_ID = _modID;
|
|
_ID = _id;
|
|
}
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
PrintTela(_ID + " ja inicializado");
|
|
return;
|
|
}
|
|
|
|
if (!mpu.init()) {
|
|
PrintTela("MPU9250 nao encontrado no endereco " + String(MPU_ADDR));
|
|
Iniciado = false;
|
|
return;
|
|
}
|
|
|
|
mpu.autoOffsets();
|
|
mpu.setSampleRateDivider(5);
|
|
mpu.setAccRange(MPU9250_ACC_RANGE_2G);
|
|
mpu.enableAccDLPF(true);
|
|
mpu.setAccDLPF(MPU9250_DLPF_6);
|
|
|
|
if (!bmp.begin(BMP_ADDR)) {
|
|
PrintTela("BMP280 nao encontrado no endereco " + String(BMP_ADDR));
|
|
Iniciado = false;
|
|
return;
|
|
}
|
|
|
|
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
|
|
Adafruit_BMP280::SAMPLING_X2,
|
|
Adafruit_BMP280::SAMPLING_X16,
|
|
Adafruit_BMP280::FILTER_X16,
|
|
Adafruit_BMP280::STANDBY_MS_500);
|
|
|
|
Iniciado = true;
|
|
PrintTela(_ID + " iniciado");
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado) {
|
|
PrintTela(_ID + " nao esta inicializado");
|
|
return;
|
|
}
|
|
|
|
PrintTela(_ID + " Desligado");
|
|
|
|
Iniciado = false;
|
|
}
|
|
|
|
void RequisitarDados() {
|
|
AferirDados();
|
|
}
|
|
|
|
std::vector<uint8_t> MontarMensagemCAN() {
|
|
std::vector<uint8_t> data;
|
|
data.push_back(ID_Num); // Byte 0 - ID
|
|
data.push_back(Iniciado ? 1 : 0); // Byte 1 - Status
|
|
|
|
// Exemplo com AccX, AccY, Temp, Pressao, Altitude
|
|
int16_t accX_int = static_cast<int16_t>(AccX * 100);
|
|
int16_t accY_int = static_cast<int16_t>(AccY * 100);
|
|
int16_t temp_int = static_cast<int16_t>(Temp * 100);
|
|
uint16_t pressao_int = static_cast<uint16_t>(Pressao / 10);
|
|
int16_t alt_int = static_cast<int16_t>(Altitude * 10);
|
|
|
|
data.push_back(accX_int >> 8); data.push_back(accX_int & 0xFF);
|
|
data.push_back(accY_int >> 8); data.push_back(accY_int & 0xFF);
|
|
data.push_back(temp_int >> 8); data.push_back(temp_int & 0xFF);
|
|
data.push_back(pressao_int >> 8); data.push_back(pressao_int & 0xFF);
|
|
data.push_back(alt_int >> 8); data.push_back(alt_int & 0xFF);
|
|
|
|
return data;
|
|
}
|
|
|
|
private:
|
|
void AferirDados() {
|
|
if (!Iniciado) return;
|
|
|
|
xyzFloat acc = mpu.getGValues();
|
|
xyzFloat gyr = mpu.getGyrValues();
|
|
xyzFloat mag = mpu.getMagValues();
|
|
|
|
AccX = acc.x;
|
|
AccY = acc.y;
|
|
AccZ = acc.z;
|
|
|
|
GyroX = gyr.x;
|
|
GyroY = gyr.y;
|
|
GyroZ = gyr.z;
|
|
|
|
MagX = mag.x;
|
|
MagY = mag.y;
|
|
MagZ = mag.z;
|
|
|
|
Temp = bmp.readTemperature();
|
|
Pressao = bmp.readPressure();
|
|
Altitude = bmp.readAltitude();
|
|
}
|
|
};
|
|
|
|
#endif
|