Overshooting and slow stabilisation pitch value

[Gabriele-Cano]

Hi, I'm having some trouble getting relaiable and precise values from an ICM42688 using the Fusion algorithm on an ESP32-C3 (custom PCB). I implemeted Fusion algorithm following the Simple example, the pitch value is then printed on serial port to PC where a Python program shows a graph of that (white line).

My PCB is attached to a slab that can oscillate from one position to another. During the test i was moving the slab in a way that should output a "square wave" of the pitch value (in red the output i was expecting). As you can see in the picture there is quite a bit of overshoot and the pitch value takes some seconds to adjust to the correct value.

I believe this as something to do with the way i move the slab, infact I slam it from one side to the other quite aggressively (this is to simulate the final use).

Can someone help me solving this problem? Thank you!

#include <Arduino.h>
#include "ICM42688.h"
#include <math.h>
#include <Fusion.h>
#include <stdbool.h>
#include <stdio.h>

// ICM42688 object with the sensor on I2C bus 0 with address 0x68
ICM42688 IMU(Wire, 0x68);

#define SAMPLE_PERIOD (0.07f) // replace this with actual sample period

FusionAhrs ahrs;

void setup() {
  Serial.begin(115200);

  // Configure I2C pins
  Wire.begin(5, 6);

  // Start communication with IMU
  delay(500);
  int status = IMU.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while(1) {
      Serial.println("IMU initialization unsuccessful");
      delay(100);
    }
  }

  FusionAhrsInitialise(&ahrs);
}

void loop() {
  // Read the sensor data
  IMU.getAGT();

  // Get accelerometer and gyroscope data
  float ax = IMU.accX();
  float ay = IMU.accY();
  float az = IMU.accZ();
  float gx = IMU.gyrX();// * M_PI / 180.0f; // Convert to radians
  float gy = IMU.gyrY();// * M_PI / 180.0f;
  float gz = IMU.gyrZ();// * M_PI / 180.0f;

  const FusionVector gyroscope = {gx, gy, gz}; // replace this with actual gyroscope data in degrees/s
  const FusionVector accelerometer = {ax, ay, az}; // replace this with actual accelerometer data in g

  FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, SAMPLE_PERIOD);
  const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
  if (Serial.available() > 0) {
    Serial.printf("%0.2f\n", euler.angle.pitch, euler.angle.yaw);
  }
  delay(70);
}