Closed black closed 1 year ago
Change that line to return 'f' Tell me if it works!
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;
}
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;
}
Nope you shouldn't remove that line, just return conv.f Tell me if it works
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);
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
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.
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);
}