0

I'm calculating angles using an IMU which as you may know involves a lot of maths. In-order to get angles at any moment, a time stamp of each reading is applied which is used to determine the refresh rate/update rate which i used in the equation.

The idea is to get a value which i can use as the rate relative to a whole second(1000 ms),i do-: 1 / (1000 / TimeStamp difference) / gyro Scale value which results in a very small decimal when done by hand, doing this with C++ yields 0 every time which is confusing because its in the exact order i should do it in.

Thanks in advance;

Kelvin

#include "stdafx.h"

typedef struct {
    double Accel_X;                      /*!< Accelarometer value X axis */
    double Accel_Y;                      /*!< Accelarometer value Y axis */
    double Accel_Z;                      /*!< Accelarometer value Z axis */

    double Gyroscope_X;              /*!< Gyroscope value X axis */
    double Gyroscope_Y;              /*!< Gyroscope value Y axis */
    double Gyroscope_Z;              /*!< Gyroscope value Z axis */

    __int16 Temperature;         /*!< Temperature in degrees */
    __int32 TimeStamp;               /*!< Time in Ms when measurement was taken */

    bool isPopulated;    /*!< Indicates if the current instance has been populated */
} MPU6050_Raw_Result_Calib;

MPU6050_Raw_Result_Calib LatestResultRaw_Calib[2];

float xAngle;
double gyroScale = 65.5;

double calc()
{
    double actualRate;

    if (!LatestResultRaw_Calib[0].isPopulated && !LatestResultRaw_Calib[1].isPopulated)
        return 0;

    __int16 refreshRate = LatestResultRaw_Calib[0].TimeStamp - LatestResultRaw_Calib[1].TimeStamp;

    actualRate = 1 / (1000 / refreshRate) / gyroScale;

    xAngle += LatestResultRaw_Calib[0].Gyroscope_X * actualRate;

    return xAngle;
}

int main()
{
    LatestResultRaw_Calib[0].Gyroscope_X = 1;
    LatestResultRaw_Calib[0].isPopulated = true;
    LatestResultRaw_Calib[0].TimeStamp = 1565;

    LatestResultRaw_Calib[1].Gyroscope_X = 15;
    LatestResultRaw_Calib[1].isPopulated = true;
    LatestResultRaw_Calib[1].TimeStamp = 1500;

    while (1)
    {
        printf("%f ", calc());
    }
}
kelvinmac
  • 191
  • 1
  • 15

1 Answers1

-1

In your equations and definitions, use a double format ie. 1.0, 1565.0, and so on

Matthew Wherry
  • 343
  • 2
  • 11