Initial commit

arduino-firmware.ino

@@ -0,0 +1,92 @@
+#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);
+  }*/
+}