Sparkfun ICM20948 Yaw Problem

Other Hardware Sensors
1

Gentlemen,

I tried out the SparkFun ICM20948 library and did angle testing with my code and it worked out really nicely. However, something I noticed with the yaw is that it starts from 0 and starts climbing to a random number, which is not exactly what I want. Is there some sort of tare functionality or an angle-presetting functionality I can use to solve this issue. Here is the code and here is the problem modelled on a video:

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "ICM_20948.h"
#include "SimpleKalmanFilter.h" // Include Kalman filter library

#define SERIAL_PORT Serial
#define WIRE_PORT Wire 
#define AD0_VAL 0

ICM_20948_I2C myICM;

// Initialize Kalman filters for roll, pitch, and yaw
SimpleKalmanFilter kalmanRoll(2, 2, 0.01);
SimpleKalmanFilter kalmanPitch(2, 2, 0.01);
SimpleKalmanFilter kalmanYaw(2, 2, 0.01);


// Motor A connections
#define enA 13
#define in1 11
#define in2 12
// Motor B connections
#define enB 8
#define in3 10
#define in4 9

const byte MOTOR_A = 3;  
const byte MOTOR_B = 2;  

float stepcount = 275.00;
float circumference = 20.00;

volatile int counter_A = 0;
volatile int counter_B = 0;

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire1, OLED_RESET);

void setup() {
  attachInterrupt(digitalPinToInterrupt (MOTOR_A), ISR_countA, RISING); // Attach interupts for encoders
  attachInterrupt(digitalPinToInterrupt (MOTOR_B), ISR_countB, RISING);
  SERIAL_PORT.begin(115200);
  WIRE_PORT.begin();
  WIRE_PORT.setClock(400000);
  Wire1.begin();
  Wire1.setClock(400000);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
	// Set all the motor control pins to outputs
	pinMode(enA, OUTPUT);
	pinMode(enB, OUTPUT);
	pinMode(in1, OUTPUT);
	pinMode(in2, OUTPUT);
	pinMode(in3, OUTPUT);
	pinMode(in4, OUTPUT);
	
	// Turn off motors - Initial state
	digitalWrite(in1, LOW);
	digitalWrite(in2, LOW);
	digitalWrite(in3, LOW);
	digitalWrite(in4, LOW);
  delay(2000);

  bool initialized = false;
  while (!initialized) {
    myICM.begin(WIRE_PORT, AD0_VAL);
    if (myICM.status == ICM_20948_Stat_Ok) initialized = true;
    else delay(500);
  }

  myICM.initializeDMP();
  myICM.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR);
  myICM.setDMPODRrate(DMP_ODR_Reg_Quat6, 0);
  myICM.enableFIFO();
  myICM.enableDMP();
  myICM.resetDMP();
  myICM.resetFIFO();
  
  
  while (digitalRead(40) == LOW) {
    measureAngle();
  }
  directionControl(444, 150);
  
  
}

void loop() {
  //ok
}

// This function lets you control spinning direction of motors
void directionControl(int steps, int speed) {
  counter_A = 0;
  counter_B = 0;
  while ((2*steps) > counter_B + counter_A) {
    	// Set motors to maximum speed
	  // For PWM maximum possible values are 0 to 255
	  analogWrite(enA, speed*0.95); // Set PWM to 105
	  analogWrite(enB, speed);

	  // Turn on motor A & B
	  digitalWrite(in1, HIGH); // Move Motors Forward
	  digitalWrite(in2, LOW);
	  digitalWrite(in3, HIGH);
	  digitalWrite(in4, LOW);
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0, 0);
    display.print(" Counter A: ");
    display.println(counter_A);
    display.print(" Counter B: ");
    display.println(counter_B);
    display.display();
	  delay(10);
    }
	digitalWrite(in1, LOW); // Stop motors
	digitalWrite(in2, LOW);
	digitalWrite(in3, LOW);
	digitalWrite(in4, LOW);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print(" Counter A: ");
  display.println(counter_A);
  display.print(" Counter B: ");
  display.println(counter_B);
  display.print(" SpeedA: ");
  display.println(speed);
  display.print(" SpeedB: ");
  display.println(speed*0.95);
  display.display();



}





void measureAngle() {
  icm_20948_DMP_data_t data;
  myICM.readDMPdataFromFIFO(&data);

  if ((myICM.status == ICM_20948_Stat_Ok) || (myICM.status == ICM_20948_Stat_FIFOMoreDataAvail)) {
    double q1 = ((double)data.Quat6.Data.Q1) / 1073741824.0;
    double q2 = ((double)data.Quat6.Data.Q2) / 1073741824.0;
    double q3 = ((double)data.Quat6.Data.Q3) / 1073741824.0;
    double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));

    double qw = q0, qx = q2, qy = q1, qz = -q3;
    double t0 = +2.0 * (qw * qx + qy * qz);
    double t1 = +1.0 - 2.0 * (qx * qx + qy * qy);
    double roll = atan2(t0, t1) * 180.0 / PI;

    double t2 = +2.0 * (qw * qy - qx * qz);
    t2 = constrain(t2, -1.0, 1.0);
    double pitch = asin(t2) * 180.0 / PI;

    double t3 = +2.0 * (qw * qz + qx * qy);
    double t4 = +1.0 - 2.0 * (qy * qy + qz * qz);
    double yaw = atan2(t3, t4) * 180.0 / PI;

    // Apply Kalman filtering
    roll = kalmanRoll.updateEstimate(roll);
    pitch = kalmanPitch.updateEstimate(pitch);
    yaw = kalmanYaw.updateEstimate(yaw);

    SERIAL_PORT.print(F("Filtered Roll:"));
    SERIAL_PORT.print(roll, 1);
    SERIAL_PORT.print(F(" Pitch:"));
    SERIAL_PORT.print(pitch, 1);
    SERIAL_PORT.print(F(" Yaw:"));
    SERIAL_PORT.println(yaw, 1);
  }
}


void ISR_countA()  {
  counter_A++;  // increment Motor A counter value
} 

// Motor B pulse count ISR
void ISR_countB()  {
  counter_B++;  // increment Motor B counter value
}
2

Magnetometers don't work "out of the box" and must be calibrated before they can be used as a compass.

I'm not familiar with the DMP code you've posted, but I suspect that without studying comprehensive documentation on how the DMP works and is supposed to be used, running it is a waste of time.

There is plenty of open source AHRS or electronic compass code for that chip that IS well documented.

Best overview and tutorial on magnetometer calibration: Tutorial: How to calibrate a compass (and accelerometer) with Arduino | Underwater Arduino Data Loggers

3

Thanks man!