From 4ff2db670a7058e7eeea0ea4e4f8822c8d6a9a96 Mon Sep 17 00:00:00 2001 From: Emanuele Date: Thu, 1 Dec 2022 16:20:41 +0100 Subject: [PATCH] Wrap Z angle --- src/MPU6050_light.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/MPU6050_light.cpp b/src/MPU6050_light.cpp index 738b3cd..c131852 100644 --- a/src/MPU6050_light.cpp +++ b/src/MPU6050_light.cpp @@ -205,6 +205,6 @@ void MPU6050::update(){ // https://github.com/gabriel-milan/TinyMPU6050/issues/6 angleX = wrap(filterGyroCoef*(angleAccX + wrap(angleX + gyroX*dt - angleAccX,180)) + (1.0-filterGyroCoef)*angleAccX,180); angleY = wrap(filterGyroCoef*(angleAccY + wrap(angleY + sgZ*gyroY*dt - angleAccY, 90)) + (1.0-filterGyroCoef)*angleAccY, 90); - angleZ += gyroZ*dt; // not wrapped - + angleZ += gyroZ*dt; + angleZ = wrap(angleZ, 180); }