Hi everyone,
I'm using a 9DOF IMU sensor (Adafruit LSM9DS1) for an RC vehicle. I managed to extract the sensor readings and calculate the roll and pitch angles, but I still can't calculate the yaw angle from my sensor readings. I'm getting an output from my reading but they don't make sense at this point ( differs between 0-75 or something). I have seen a lot of examples/videos calculating the yaw angle even with 6DOF sensors using only the accelerometer and the gyroscope ( for example: https://www.youtube.com/watch?v=yqFfmwVufMo&ab_channel=Matthias and https://www.youtube.com/watch?v=6OwN_HepaXY&ab_channel=square1a). How can I calculate the yaw angle preferably only using the accelerometer and the gyroscope? Here is my code, hope someone can help since I've been stuck at this point for months 
:
// Libraries
#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_LSM9DS1.h>
#include <Adafruit_Sensor.h>
// IMU filter constants
// ##########################################################
const int IMU_CALIBRATION_STEP = 1000;
const float GYRO_WEIGHT = 0.1; // Weight factor for the complementary filter
const float ALPHA_9DOF = 0.1; // Weight for accelerometer data
const float GAMMA_9DOFX = 0.2; // Weight for magnetometer data
const float GAMMA_9DOFY = 0.2; // Weight for magnetometer data
const float BETA_9DOF = 0.02; // Weight for gyroscope data
// ##########################################################
// IMU initialization
// ##########################################################
sensors_event_t a, m, g, temp;
float filteredAccelX = 0.0;
float filteredAccelY = 0.0;
float filteredMagnetometerY = 0;
float filteredMagnetometerX = 0;
float pitchAngle;
float rollAngle ;
float yawAngle ;
// ##########################################################
struct Values {
float calibratedPitchAngle;
float calibratedRollAngle;
float calibratedYawAngle;
float OFFSET_YAW;
float OFFSET_PITCH;
float OFFSET_ROLL;
bool readingGps;
};
Values readingImuValues;
Values offsets;
Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();
void setupSensor()
{
// 1.) Set the accelerometer range
lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G);
// 2.) Set the magnetometer sensitivity
// lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
//lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS);
lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS);
//lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS);
// 3.) Setup the gyroscope
lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);
//lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS);
//lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS);
}
Values calibrateOffsets() {
float pitch_sum = 0;
float roll_sum = 0;
float yaw_sum = 0;
Values values;
Serial.println("Please keep the vehicle steady.");
for (int i = 0; i < IMU_CALIBRATION_STEP; ++i) {
if (i % (IMU_CALIBRATION_STEP/5)==0){
Serial.println("Calibrating LSM9DS1...");
}
readImu();
pitch_sum += pitchAngle;
roll_sum += rollAngle;
yaw_sum += yawAngle;
delay(1);
}
// Average offsets
values.OFFSET_PITCH = pitch_sum / IMU_CALIBRATION_STEP;
values.OFFSET_ROLL = roll_sum / IMU_CALIBRATION_STEP;
values.OFFSET_YAW = yaw_sum / IMU_CALIBRATION_STEP;
Serial.println(""); Serial.println("Calibration done.");
Serial.print("\n offset pitch : "); Serial.print(values.OFFSET_PITCH); Serial.print(" offset roll : "); Serial.print(values.OFFSET_ROLL);Serial.print(" offset yaw : "); Serial.println(values.OFFSET_YAW);
// Serial.print("\n offset magnetometer X : "); Serial.print(offsetMagnetX); Serial.print(" Y : "); Serial.print(offsetMagnetY);Serial.print(" Z : "); Serial.println(offsetMagnetZ);
delay(3000);
return values;
}
void setup()
{
Serial.begin(9600);
while (!Serial) {
delay(1); // will pause Zero, Leonardo, etc until serial console opens
}
// Try to initialise and warn if we couldn't detect the chip
if (!lsm.begin())
{
Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
while (1);
}
Serial.println("Found LSM9DS1 9DOF");
// helper to just set the default scaling we want, see above!
setupSensor();
offsets = calibrateOffsets();
}
void loop()
{
readingImuValues = writeImu();
delay(50);
}
Values writeImu() {
readImu();
Values result;
result.calibratedPitchAngle= pitchAngle - offsets.OFFSET_PITCH;
result.calibratedRollAngle = rollAngle - offsets.OFFSET_ROLL;
result.calibratedYawAngle = yawAngle - offsets.OFFSET_YAW;
Serial.print("P = ");
Serial.print(result.calibratedPitchAngle);
Serial.print(" R = ");
Serial.print(result.calibratedRollAngle);
Serial.print(" Y = ");
Serial.println(result.calibratedYawAngle);
return result;
}
void readImu(){
lsm.read(); // ask it to read in the data
// Get a new sensor event
lsm.getEvent(&a, &m, &g, &temp);
// Calculte the filtered accelaration
filteredAccelY = ALPHA_9DOF * (a.acceleration.y) + (1 - ALPHA_9DOF) * filteredAccelY;
filteredAccelX = ALPHA_9DOF * (a.acceleration.x) + (1 - ALPHA_9DOF) * filteredAccelX;
float accelAngleX = (atan((filteredAccelY)/sqrt(pow((filteredAccelX),2) + pow((a.acceleration.z),2)))*RAD_TO_DEG);
float accelAngleY = (atan(-1*(filteredAccelX)/sqrt(pow((filteredAccelY),2) + pow((a.acceleration.z),2)))*RAD_TO_DEG);
// Complementary filter to combine accelerometer and gyroscope data
pitchAngle = GYRO_WEIGHT * (g.gyro.x + pitchAngle) + (1-GYRO_WEIGHT) * accelAngleX;
rollAngle = GYRO_WEIGHT * (g.gyro.y + rollAngle) + (1-GYRO_WEIGHT) * accelAngleY;
// Calculate yaw angle from magnetometer data
filteredMagnetometerY = GAMMA_9DOFY * (m.magnetic.y) + (1 - GAMMA_9DOFY) * filteredMagnetometerY;
filteredMagnetometerX = GAMMA_9DOFX * (m.magnetic.x) + (1 - GAMMA_9DOFX) * filteredMagnetometerX;
yawAngle = atan2(filteredMagnetometerY, filteredMagnetometerX) * RAD_TO_DEG;
}```