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());
}
}