// d1 mini side (transmitter)
#include <Wire.h>
#include <MPU6050_light.h>
MPU6050 mpu(Wire);
// state
bool isConnected = false;
const int led = 23;
// Kalman filter variables
double angle = 0.0; // Current angle estimate
double angleEstimate = 0.0; // Initial angle estimate
double angleCovariance = 1.0; // Initial covariance estimate
// Kalman filter update step
void kalman_update(double measurement) {
// Prediction step
double prediction = angleEstimate;
double predictionCovariance = angleCovariance + 0.1; // Process noise
// Calculate Kalman gain
double kalmanGain = predictionCovariance / (predictionCovariance + 1.0); // Measurement noise
// Update step
angle = prediction + kalmanGain * (measurement - prediction);
angleCovariance = (1.0 - kalmanGain) * predictionCovariance;
angleEstimate = angle;
}
void setup() {
Serial.begin(115200);
pinMode(led, OUTPUT);
digitalWrite(led, LOW);
// polling
// while (true) {
// // sending test message
// Serial.println("ping");
// if (Serial.available() > 0) {
// String inp = Serial.readStringUntil('\n');
// inp.trim();
// if (inp == "pong") {
// break;
// } //connection established
// }
// delay(100);
// }
// starting mpu
Wire.begin();
while (mpu.begin()) {
digitalWrite(led, !(digitalRead(led)));
delay(100);
}
// calibrating
digitalWrite(led, HIGH);
delay(500);
// mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
mpu.calcOffsets(); // gyro and accelero
digitalWrite(led, LOW);
}
void loop() {
// get data from mpu
mpu.update();
double zangle = mpu.getAngleZ();
// pass through kalman filter (to provide more accurate results)
kalman_update(zangle);
// print to serial
Serial.println(angle);
delay(50);
}