Convert sketch from double to float

Hi,
I attempted to convert my sketch from double to float precision, Can you check if it’s ok?

This is float:

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").

 Contact information
 -------------------

 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

#include <Wire.h>
#include <Mouse.h>

// Source: https://github.com/TKJElectronics/KalmanFilter

#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf


/* IMU Data */
float accX, accY, accZ;
float gyroX, gyroY, gyroZ;
int16_t tempRaw;
int16_t xstep;
int16_t ystep;
int16_t divider;


float gyroXangle, gyroYangle; // Angle calculate using the gyro only
float compAngleX, compAngleY; // Calculated angle using a complementary filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

// TODO: Make calibration routine

void setup() {
  
  Serial.begin(115200);
  Wire.begin();
  Mouse.begin();
  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  delay(100); // Wait for sensor to stabilize

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = (i2cData[0] << 8) | i2cData[1];
  accY = (i2cData[2] << 8) | i2cData[3];
  accZ = (i2cData[4] << 8) | i2cData[5];

  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  float roll  = atan2f(accY, accZ) * RAD_TO_DEG;
  float pitch = atanf(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  float roll  = atanf(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  float pitch = atan2f(-accX, accZ) * RAD_TO_DEG;
#endif


  gyroXangle = roll;
  gyroYangle = pitch;
  compAngleX = roll;
  compAngleY = pitch;

  timer = micros();
}

void loop() {
 
  /* Update all the values */
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = (i2cData[6] << 8) | i2cData[7];
  gyroX = (i2cData[8] << 8) | i2cData[9];
  gyroY = (i2cData[10] << 8) | i2cData[11];
  gyroZ = (i2cData[12] << 8) | i2cData[13];

  float dt = (float)(micros() - timer) / 1000000; // Calculate delta time
  timer = micros();
  

  // Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
  float roll  = atan2f(accY, accZ) * RAD_TO_DEG;
  float pitch = atanf(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
  float roll  = atanf(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
  float pitch = atan2f(-accX, accZ) * RAD_TO_DEG;
#endif

  float gyroXrate = gyroX / 131.0; // Convert to deg/s
  float gyroYrate = gyroY / 131.0; // Convert to deg/s

  

  gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * dt;
  //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate() * dt;

  compAngleX = 0.98 * (compAngleX + gyroXrate * dt) + 0.02 * roll; // Calculate the angle using a Complimentary filter
  compAngleY = 0.98 * (compAngleY + gyroYrate * dt) + 0.02 * pitch;

  // Reset the gyro angle when it has drifted too much
if (millis()> refreshrate +100){
  refreshrate= millis();
xstep = compAngleX;
ystep = compAngleY;
}  
divider = 2;
 Serial.print(xstep);
  Serial.print(ystep);

Which board are you compiling for?

pert:
Which board are you compiling for?

Thanks for reply. Right now it's for a Leonardo, but i intend to use it in a Itsy Bitsy m4 that i will get in a few days.

amadeok:
Thanks for reply. Right now it's for a Leonardo, but i intend to use it in a Itsy Bitsy m4 that i will get in a few days.

On most Arduinos, a double and a float are the same size.

This is NOT the Itsy Bitsy m4 forum.

In Arduino UNO, the float and double data types offer the same accuracy of 6-digit after the decimal point.

In Arduino DUE, float offers 6-digit accuracy and double offers 15-digit accuracy.

The Adafruit Itsy Bitsy M4 uses the ATSAMD51 microcontroller. It requires the Adafruit SAMD Boards core, which is a modified version of Arduino SAMD Boards.

// Source: GitHub - TKJElectronics/KalmanFilter: This is a Kalman filter used to calculate the angle, rate and bi

As I mentioned earlier, that library doesn't work. You are wasting your time.

pert:
The Adafruit Itsy Bitsy M4 uses the ATSAMD51 microcontroller. It requires the Adafruit SAMD Boards core, which is a modified version of Arduino SAMD Boards.

Sorry i'm not sure what you mean by that. I know that the Itsy Bitsy m4 has a floating point unit but works only with float.

jremington:
As I mentioned earlier, that library doesn't work. You are wasting your time.

If you look at the code you'll see that i already deleted the code related to Kalman (at least most of it).

amadeok:
Sorry i'm not sure what you mean by that.

You'll find out when you get the board and want to make it work with the Arduino IDE:

Well, I ran it through a build, in the hopes of being able to dump the symbols looking for dXXXX function names indicating that you had inadvertently done an operation with a double...

It's not very close to compiling :frowning: So I didn't get very far.

i2cWrite() and i2cRead() don't seem to exist. They don't seem to exist for Leonardo, either. The wire.h library is all about "twiXXXX", rather than "i2cXXXX", so I'm not sure where these are supposed to come from. (maybe they're later in your program - the code looks truncated.)

sqrt() should be sqrtf()

pert:
You’ll find out when you get the board and want to make it work with the Arduino IDE:
Arduino IDE Setup | Introducing Adafruit ItsyBitsy M4 | Adafruit Learning System

Yes the Adafruit support already provided me with that link, and followed the guide and compiled my sketch for the Itsy Bitsy and after changing the TWBR command with Wire.setClock() it did compile. Now i hope it works.

westfw:
Well, I ran it through a build, in the hopes of being able to dump the symbols looking for dXXXX function names indicating that you had inadvertently done an operation with a double…

It’s not very close to compiling :frowning: So I didn’t get very far.

i2cWrite() and i2cRead() don’t seem to exist. They don’t seem to exist for Leonardo, either. The wire.h library is all about “twiXXXX”, rather than “i2cXXXX”, so I’m not sure where these are supposed to come from. (maybe they’re later in your program - the code looks truncated.)

sqrt() should be sqrtf()

Sorry i’m ashamed. The fellow that made most of the code related to the MPU also provided the files i attached( kalman.h is irrelevant since i’m not using that kalman filter, as it’s been pointed out its not good) This is the github link: KalmanFilter/examples/MPU6050 at master · TKJElectronics/KalmanFilter · GitHub
It does compile with those files.

I2C.ino (2.3 KB)

Kalman.h (4.24 KB)

In addition to the sqrt() function, all of your constants need to be converted to be explicitly single precision values, or they will be "assumed" to be DP and cause the whole expression to be evaluated in DP.
That means stuff like:

  float roll  = atanf(accY / sqrtf(accX * accX + accZ * accZ)) * [color=red](float)RAD_TO_DEG[/color];
    :
  float gyroXrate = gyroX / [color=red]131.0f[/color]; // Convert to deg/s
   :
  compAngleX = [color=red]0.98f[/color] * (compAngleX + gyroXrate * dt) + [color=red]0.02f[/color] * roll; // Calculate the angle using a

Aside from that, it looks pretty good. I ran the code through the dis-assembler looking for DP math calls, and the only ones I found were from print() and exception handling in sqrtf() (which is weird, and probably a bug, but shouldn't affect performance.)