/*
* Return an estimate in radians of the angle being made with the ground
*/
float getAngleEstimate() {
getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// Get Gyro Value
angleG = getAngularVelocity();
angleA = getAngularAcceleration();
// State estimation
angleEstimate = HPF * (angleEstimate + angleG * DT) + LPF * angleA;
return angleEstimate;
}
void MPU6050::Update()
{
while(m_Timer.read() < .003) {}
//Get sensor data
getMotion6(m_ax, m_ay, m_az,m_gx, m_gy, m_gz);
//Applying offset and Scaler to acelerometer data
m_az = (m_az + 272)* .9674953;
m_ay = (m_ay - 13)* .9873171;
m_ax = (m_ax - 51)* .98828125;
//Converting raw acelerometer to usable data
m_AX = (m_ax*(16.0/32768.0));
m_AY = (m_ay *(16.0/32768.0));
m_AZ = (m_az*(16.0/32768.0));
//Turn raw Gyro data into usable data
m_GX = (((m_gx - 220) * .005) * .263);// * 1.34 ) * .001;
m_GY = (((m_gy + 26) * .005) * .261);//*5.1021854) * .001;
m_GZ = (((m_gz + 26) * .005) * .261);
//Floor the Gyro data if change is less than .05
if(m_GX > .05 || m_GX < -.05)
m_Roll += m_GX;
if(m_GY > .05 || m_GY < -.05)
m_Pitch += m_GY;
if(m_GZ > .05 || m_GZ < -.05)
m_Yaw += m_GZ;
//Get accelerometer pitch and roll
m_pitchAcc = ((atan2(m_AX, sqrt(m_AY*m_AY + m_AZ*m_AZ)) * 180) / 3.14159265359);
if(m_pitchAcc < 75)
m_rollAcc = ((atan2(m_AY, m_AZ) * 180 / 3.14159265359) - 1.469697);
//Get Combined pitch and roll
m_CalibratedRoll = m_kRoll.KalmanCalculate(m_rollAcc, m_Roll, m_Timer.read());
if(m_CalibratedRoll <= 0)
{
m_CalibratedRoll += 360.0f;
}
m_CalibratedPitch = m_kPitch.KalmanCalculate(m_pitchAcc, m_Pitch, m_Timer.read());
//Reset timer to insure a .003 second delay between reads
m_Timer.reset();
}
void CurieIMUClass::readMotionSensor(int& ax, int& ay, int& az, int& gx, int& gy, int& gz)
{
short sax, say, saz, sgx, sgy, sgz;
getMotion6(&sax, &say, &saz, &sgx, &sgy, &sgz);
ax = sax;
ay = say;
az = saz;
gx = sgx;
gy = sgy;
gz = sgz;
}
void CurieImuClass::readMotionSensorScaled(float &ax, float &ay, float &az,
float &gx, float &gy, float &gz)
{
int16_t sax, say, saz, sgx, sgy, sgz;
getMotion6(&sax, &say, &saz, &sgx, &sgy, &sgz);
ax = convertRaw(sax, accel_range);
ay = convertRaw(say, accel_range);
az = convertRaw(saz, accel_range);
gx = convertRaw(sgx, gyro_range);
gy = convertRaw(sgy, gyro_range);
gz = convertRaw(sgz, gyro_range);
}
void burnin(){
int i=BURNIN;
P1DIR |= BIT0;
P1OUT |= BIT0;
while(i--){
if (i%10==0) {
P1OUT^=BIT0;
}
getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//ax_bias += (float)ax*BURNIN_DATA;
//az_bias += (float)az*BURNIN_DATA;
gy_bias += (float)gy*BURNIN_DATA;
atan2_bias += atan2(ax, az) * BURNIN_DATA;
}
P1OUT &= ~BIT0;
}
void MPU6050::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) {
getMotion6(ax, ay, az, gx, gy, gz);
// TODO: magnetometer integration
}
/**
* get yaw, pitch and roll information
*
* @param yprAttitude
* yaw, pitch and roll
*
* @param yprRate
* rate
*
* @param xyzAcc
* acceleration
*
* @param xyzGravity
* gravity
*
* @param xyzMagnet
* magnet
*
* @return
* void
*
*/
void getYawPitchRollInfo(float *yprAttitude, float *yprRate,
float *xyzAcc, float *xComponent, float *yComponent, float *zComponent, float *xyzMagnet) {
float q[4]; // [w, x, y, z] quaternion container
float mXComponent[3];
float mYComponent[3];
float mZComponent[3];
float ax = 0.f;
float ay = 0.f;
float az = 0.f;
float gx = 0.f;
float gy = 0.f;
float gz = 0.f;
#ifdef MPU6050_9AXIS
short s_mx=0;
short s_my=0;
short s_mz=0;
float f_mx=0.f;
float f_my=0.f;
float f_mz=0.f;
float f_x=0.f;
float f_y=0.f;
float f_z=0.f;
#endif
getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
#ifdef MPU6050_9AXIS
if(pollingMagnetDataBySingleMeasurementMode(&s_mx, &s_my, &s_mz)){
f_x = (float)s_mx - mag_hard_iron_cal[0];
f_y = (float)s_my - mag_hard_iron_cal[1];
f_z = (float)s_mz - mag_hard_iron_cal[2];
f_mx = f_x * mag_soft_iron_cal[0][0] + f_y * mag_soft_iron_cal[0][1] + f_z * mag_soft_iron_cal[0][2];
f_my = f_x * mag_soft_iron_cal[1][0] + f_y * mag_soft_iron_cal[1][1] + f_z * mag_soft_iron_cal[1][2];
f_mz = f_x * mag_soft_iron_cal[2][0] + f_y * mag_soft_iron_cal[2][1] + f_z * mag_soft_iron_cal[2][2];
pushSmaData(&x_magnetSmaFilterEntry,f_mx);
pushSmaData(&y_magnetSmaFilterEntry,f_my);
pushSmaData(&z_magnetSmaFilterEntry,f_mz);
f_mx = pullSmaData(&x_magnetSmaFilterEntry);
f_my = pullSmaData(&y_magnetSmaFilterEntry);
f_mz = pullSmaData(&z_magnetSmaFilterEntry);
IMUupdate9(gx, gy, gz, ax, ay, az, f_my, f_mx, f_mz, q);
}else{
IMUupdate6(gx, gy, gz, ax, ay, az, q);
}
#else
IMUupdate6(gx, gy, gz, ax, ay, az, q);
#endif
GetXComponent(mXComponent, q);
GetYComponent(mYComponent, q);
GetZComponent(mZComponent, q);
GetYawPitchRoll(yprAttitude, q, mZComponent);
yprAttitude[0] = yprAttitude[0] * RA_TO_DE;
yprAttitude[1] = yprAttitude[1] * RA_TO_DE;
yprAttitude[2] = yprAttitude[2] * RA_TO_DE;
xComponent[0] = mXComponent[0];
xComponent[1] = mXComponent[1];
xComponent[2] = mXComponent[2];
yComponent[0] = mYComponent[0];
yComponent[1] = mYComponent[1];
yComponent[2] = mYComponent[2];
zComponent[0] = mZComponent[0];
zComponent[1] = mZComponent[1];
zComponent[2] = mZComponent[2];
yprRate[0] = gx * RA_TO_DE;
yprRate[1] = gy * RA_TO_DE;
yprRate[2] = gz * RA_TO_DE;
xyzAcc[0] = ax;
xyzAcc[1] = ay;
xyzAcc[2] = az;
}
