92 lines
2.5 KiB
C++
92 lines
2.5 KiB
C++
#include "Barometer.h"
|
|
#include "GyverBME280.h"
|
|
#include "Wire.h"
|
|
#include "board_pins.h"
|
|
#include "I2Cdev.h"
|
|
#include "Kalman2DFilter.h"
|
|
#include "MPU6050_6Axis_MotionApps20.h"
|
|
#include "BatteryController.h"
|
|
#include "MPU.h"
|
|
|
|
TwoWire i2c = TwoWire(0);
|
|
Barometer barometer(new GyverBME280(i2c));
|
|
MPU mpu(new MPU6050(MPU6050_DEFAULT_ADDRESS, &i2c));
|
|
Kalman2DFilter kalman2d(10.f, 1.f, 1.8f);
|
|
BatteryController battery;
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
i2c.begin(I2C_SDA_PIN, I2C_SCL_PIN, 400000);
|
|
Serial.println(barometer.initialize());
|
|
Serial.println(mpu.initialize());
|
|
barometer.measureBaseAltitudeSync();
|
|
battery.initialize();
|
|
Serial.println("System initialized");
|
|
}
|
|
|
|
void loop() {
|
|
/*Serial.print("ax:");
|
|
Serial.print(ax);
|
|
Serial.print(",");
|
|
Serial.print("ay:");
|
|
Serial.print(ay);
|
|
Serial.print(",");
|
|
Serial.print("az:");
|
|
Serial.print(az);
|
|
Serial.print(",");*/
|
|
bool err;
|
|
barometer.tick();
|
|
if(mpu.tick(err) && !err) {
|
|
float kalmanAltitude, ZVelocityAltitude;
|
|
float barometerAltitude = barometer.rawFlightHeight();
|
|
kalman2d.filter(mpu.accZInertial(), barometerAltitude, kalmanAltitude, ZVelocityAltitude);
|
|
//Serial.print("Yaw:");
|
|
//Serial.print(mpu.yaw());
|
|
//Serial.print(",");
|
|
//Serial.print("Roll:");
|
|
//Serial.print(mpu.roll());
|
|
//Serial.print(",");
|
|
//Serial.print("Pitch:");
|
|
//Serial.print(mpu.pitch());
|
|
//Serial.print(",");
|
|
//Serial.print("Flight height:");
|
|
Serial.print(barometerAltitude);
|
|
Serial.print(",");
|
|
//Serial.print("Kalman altitude:");
|
|
Serial.print(kalmanAltitude);
|
|
Serial.print(",");
|
|
/*Serial.print(G);
|
|
Serial.print(",");
|
|
Serial.print(Pc);
|
|
Serial.print(",");
|
|
Serial.print(P);
|
|
Serial.print(",");
|
|
Serial.print(Xp);
|
|
Serial.print(",");*/
|
|
//Serial.print("Kalman2 altitude: ");
|
|
Serial.println(ZVelocityAltitude);
|
|
/*Serial.print("Vertical velocity:");
|
|
Serial.print(ZVelocityAltitude);
|
|
Serial.print(",");
|
|
Serial.print("Battery percent: ");
|
|
Serial.println(battery.percent());*/
|
|
}
|
|
|
|
//Serial.print(",");
|
|
/*
|
|
//Serial.print("AccZInertial:");
|
|
Serial.print(AccZInertial);
|
|
Serial.print(",");
|
|
//Serial.print("Altitude:");
|
|
Serial.print(AltitudeBarometer);
|
|
Serial.print(",");
|
|
//Serial.print("kAltitude:");
|
|
Serial.print(AltitudeKalman);
|
|
Serial.print(",");
|
|
//Serial.print("Battery voltage: ");
|
|
//Serial.print(battery.measureVoltage());
|
|
//Serial.print(",");
|
|
//Serial.print("kZvelocity:");
|
|
Serial.println(VelocityVerticalKalman);
|
|
}*/
|
|
} |