124 lines
3.2 KiB
C++
124 lines
3.2 KiB
C++
#include <Wire.h>
|
|
#include <MPU9250_WE.h>
|
|
#include <Adafruit_BMP280.h>
|
|
#include <MadgwickAHRS.h>
|
|
|
|
#include <MPU9250_asukiaaa.h>
|
|
|
|
MPU9250_asukiaaa mySensor;
|
|
|
|
// Endereços típicos dos sensores
|
|
#define ADDR_MPU 0x68
|
|
#define ADDR_BMP 0x76
|
|
|
|
MPU9250_WE mpu = MPU9250_WE(ADDR_MPU);
|
|
Adafruit_BMP280 bmp;
|
|
Madgwick filter;
|
|
|
|
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;
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
delay(500);
|
|
Wire.begin(1, 2);
|
|
|
|
Serial.println("🔍 Iniciando Scanner I2C...");
|
|
for (byte address = 1; address < 127; address++) {
|
|
Wire.beginTransmission(address);
|
|
if (Wire.endTransmission() == 0) {
|
|
Serial.print("📍 Dispositivo encontrado no endereco 0x");
|
|
Serial.println(address, HEX);
|
|
}
|
|
}
|
|
Serial.println("✅ Scanner finalizado.\n");
|
|
|
|
Serial.println("🎯 Iniciando sensores...");
|
|
|
|
Serial.print("WHO_AM_I: 0x");
|
|
Serial.println(mpu.whoAmI(), HEX);
|
|
|
|
mySensor.setWire(&Wire);
|
|
mySensor.beginAccel();
|
|
mySensor.beginGyro();
|
|
mySensor.beginMag(); // tenta usar AK8963 se existir
|
|
|
|
/*bool mpuIniciado = mpu.init();
|
|
// MPU9250
|
|
if (!mpuIniciado) {
|
|
Serial.println("MPU9250 NAO encontrado!");
|
|
} else {
|
|
mpu.autoOffsets();
|
|
mpu.setSampleRateDivider(5);
|
|
mpu.setAccRange(MPU9250_ACC_RANGE_2G);
|
|
mpu.enableAccDLPF(true);
|
|
mpu.setAccDLPF(MPU9250_DLPF_6);
|
|
Serial.println("MPU9250 OK");
|
|
}*/
|
|
|
|
// BMP280
|
|
if (!bmp.begin(ADDR_BMP)) {
|
|
Serial.println("BMP280 NAO encontrado!");
|
|
} else {
|
|
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
|
|
Adafruit_BMP280::SAMPLING_X2,
|
|
Adafruit_BMP280::SAMPLING_X16,
|
|
Adafruit_BMP280::FILTER_X16,
|
|
Adafruit_BMP280::STANDBY_MS_500);
|
|
Serial.println("BMP280 OK");
|
|
}
|
|
|
|
// Madgwick
|
|
filter.begin(100); // 100 Hz
|
|
}
|
|
|
|
void loop() {
|
|
// Atualizar dados do MPU
|
|
/*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;*/
|
|
|
|
mySensor.accelUpdate();
|
|
mySensor.gyroUpdate();
|
|
mySensor.magUpdate(); // pode falhar se não tiver magnetômetro
|
|
|
|
AccX = mySensor.accelX(); AccY = mySensor.accelY(); AccZ = mySensor.accelZ();
|
|
GyroX = mySensor.gyroX(); GyroY = mySensor.gyroY(); GyroZ = mySensor.gyroZ();
|
|
MagX = mySensor.magX(); MagY = mySensor.magY(); MagZ = mySensor.magZ();
|
|
|
|
// Atualizar dados do BMP
|
|
Temp = bmp.readTemperature();
|
|
Pressao = bmp.readPressure();
|
|
Altitude = bmp.readAltitude();
|
|
|
|
// Filtro Madgwick
|
|
filter.update(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, MagX, MagY, MagZ);
|
|
Roll = filter.getRoll();
|
|
Pitch = filter.getPitch();
|
|
Yaw = filter.getYaw();
|
|
|
|
// Exibir no Serial
|
|
Serial.println("---- AMOSTRAGEM ----");
|
|
Serial.print("Temp: "); Serial.println(Temp);
|
|
Serial.print("Pressao: "); Serial.println(Pressao);
|
|
Serial.print("Altitude: "); Serial.println(Altitude);
|
|
|
|
Serial.print("Roll: "); Serial.println(Roll);
|
|
Serial.print("Pitch: "); Serial.println(Pitch);
|
|
Serial.print("Yaw: "); Serial.println(Yaw);
|
|
Serial.println();
|
|
|
|
delay(1000);
|
|
}
|