SensorFusion.cpp:67:9: error: 'y' was not declared in this scope return y;

[black]

c:\Users\black\Documents\Arduino\libraries\SensorFusion-1.0.5\src\SensorFusion.cpp: In static member function 'static float SF::invSqrt(float)':
c:\Users\black\Documents\Arduino\libraries\SensorFusion-1.0.5\src\SensorFusion.cpp:67:9: error: 'y' was not declared in this scope
  return y;
         ^

exit status 1

Compilation error: exit status 1

[aster94]

Change that line to return 'f' Tell me if it works!

[black]

I did make the changes but again same error

float SF::invSqrt(float x)
{
    float halfx = 0.5f * x;
    union {
      float    f;
      uint32_t i;
    } conv = { .f = x };
    conv.i = 0x5f3759df - (conv.i >> 1);
    conv.f *= 1.5f - (halfx * conv.f * conv.f);
    conv.f *= 1.5f - (halfx * conv.f * conv.f);
    return f;
}

[black]

Okay so finally I followed your comment in the code and fixed it. First it should return conv.f not f. Second you have duplicate lines in it.

float SF::invSqrt(float x)
{
    float halfx = 0.5f * x;
    union {
      float    f;
      uint32_t i;
    } conv = { .f = x };
    conv.i = 0x5f3759df - (conv.i >> 1);
    conv.f *= 1.5f - (halfx * conv.f * conv.f); 
    return conv.f;
}

[aster94]

Nope you shouldn't remove that line, just return conv.f Tell me if it works

[black]

Hey, you have these two lines duplicates. Can you explain why do we need to keep both of them?

conv.f *= 1.5f - (halfx * conv.f * conv.f);
conv.f *= 1.5f - (halfx * conv.f * conv.f);

[aster94]

Have a good read: https://en.wikipedia.org/wiki/Newton%27s_method Simply stated: two iteration decrease the error

P.S: if you really want you can remove one

[black]

Thank you again. Let me do that and get back to you. Although, I have seen a weird phenomenon while applying the sensor fusion. The graphs are coming up as saw tooth wave for one of the axis. I will share an image soon.

[black]

So I am using Seeed xiao nrf52840 sense and I am getting this serial plot and when I stream the data over bluetooth plot when sensor is just resting on the table -

// based on https://github.com/Seeed-Studio/Seeed_Arduino_LSM6DS3/blob/master/examples/HighLevelExample/HighLevelExample.ino
// BLE GATT UUID: https://btprodspecificationrefs.blob.core.windows.net/assigned-values/16-bit%20UUID%20Numbers%20Document.pdf
// https://wiki.dfrobot.com/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing#target_4
// Seeed Wiki has wrong board library for installation. It should be noted that the “mbed enabled” search result needs to be installed, not the first result
// https://github.com/aster94/SensorFusion

#include <Arduino.h>
#include <ArduinoBLE.h>

#include "LSM6DS3.h"
#include "Wire.h"
#include "SensorFusion.h"  //SF

SF fusion;

// Create a instance of class LSM6DS3
LSM6DS3 my_imu(I2C_MODE, 0x6A);  // I2C device address 0x6A

// BLE Variables
BLEService imu_service("abf0e000-b597-4be0-b869-6054b7ed0ce3");

// clock Characteristic
BLEUnsignedLongCharacteristic timer_characteristic("abf0e001-b597-4be0-b869-6054b7ed0ce3", BLERead | BLENotify);

//  Characteristic User Description Descriptor
BLEDescriptor timer_descriptor("2901", "timer_ms");
unsigned long milli_sec = 0;

// acc
BLEFloatCharacteristic pitch_characteristic("abf0e002-b597-4be0-b869-6054b7ed0ce3", BLERead | BLENotify);
BLEFloatCharacteristic yaw_characteristic("abf0e003-b597-4be0-b869-6054b7ed0ce3", BLERead | BLENotify);
BLEFloatCharacteristic roll_characteristic("abf0e004-b597-4be0-b869-6054b7ed0ce3", BLERead | BLENotify);

BLEDescriptor pitch_descriptor("2901", "pitch");
BLEDescriptor yaw_descriptor("2901", "yaw");
BLEDescriptor roll_descriptor("2901", "roll");

float pitch, roll, yaw;
float deltat;

const int LED = LED_BUILTIN;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  // build-in leds
  pinMode(LED, OUTPUT);
  // turn off the all build-in leds
  digitalWrite(LED, HIGH);

  // Call .begin() to configure the IMUs
  if (my_imu.begin() != 0) {
    Serial.println("Device error");
    digitalWrite(LED, LOW);
  } else {
    Serial.println("Device OK!");
  }

  // ble
  // begin initialization
  if (!BLE.begin()) {
    Serial.println("starting BLE failed!");
    while (1) {
      digitalWrite(LED, !digitalRead(LED));
      delay(500);
    };
  }

  BLE.setDeviceName("WearSenseIMU-SensorFusion");
  BLE.setLocalName("WearSenseIMU-SensorFusion");
  BLE.setAdvertisedService(imu_service);

  // clock
  imu_service.addCharacteristic(timer_characteristic);
  timer_characteristic.addDescriptor(timer_descriptor);

  // acc
  imu_service.addCharacteristic(pitch_characteristic);
  imu_service.addCharacteristic(yaw_characteristic);
  imu_service.addCharacteristic(roll_characteristic);

  pitch_characteristic.addDescriptor(pitch_descriptor);
  yaw_characteristic.addDescriptor(yaw_descriptor);
  roll_characteristic.addDescriptor(roll_descriptor);

  BLE.addService(imu_service);

  timer_characteristic.writeValueLE(0);

  pitch_characteristic.writeValueLE(0);
  yaw_characteristic.writeValueLE(0);
  roll_characteristic.writeValueLE(0);

  //temp_characteristic.writeValueLE(0);
  BLE.advertise();
}

void loop() {
  BLEDevice central = BLE.central();

  if (central) {
    while (central.connected()) {
      milli_sec = millis();
      timer_characteristic.writeValueLE(milli_sec);

      float ax = my_imu.readFloatAccelX();
      float ay = my_imu.readFloatAccelY();
      float az = my_imu.readFloatAccelZ();

      float gx = my_imu.readFloatGyroX();
      float gy = my_imu.readFloatGyroY();
      float gz = my_imu.readFloatGyroZ();

      //this have to be done before calling the fusion update
      deltat = fusion.deltatUpdate();

      //choose only one of these two:
      fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltat);

      pitch = fusion.getPitch();
      yaw = fusion.getYaw();
      roll = fusion.getRoll();  //you could also use getRollRadians() ecc

      pitch_characteristic.writeValueLE(pitch);
      yaw_characteristic.writeValueLE(yaw);
      roll_characteristic.writeValueLE(roll);

      Serial.print(pitch);
      Serial.print(','); 
      Serial.print(roll);
      Serial.print(','); 
      Serial.print(yaw);
      Serial.println();

      digitalWrite(LEDB, !digitalRead(LED));  // ble heartbeat
      delay(10);
    }
  }
  digitalWrite(LED, !digitalRead(LED));  // waiting ble connection
  // delay(10);
}