Pleco

IMU::IMU(Hardware *hardware):

  QObject(), hardware(hardware), enabled(false),

  yaw(0), pitch(0), roll(0),

  exInt(0), eyInt(0), ezInt(0),

  lastUpdate(0), now(0),

  halfT(0)

  gyroRaw8bit[0] = gyroRaw8bit[1] = gyroRaw8bit[2] = 0;

  magnRaw8bit[0] = magnRaw8bit[1] = magnRaw8bit[2] = 0;

}

void IMU::pushSensorData(quint8 raw8bit[9], double data[9])

{

  // Accelerometer data

  accRaw[0] = data[0];

  accRaw[1] = data[1];

 */

void IMU::doIMUCalc()

{

  // Update yaw/pitch/roll

  /*

  double q[4]; // quaternion

  double gx, gy, gz; // estimated gravity direction

  getQ(q);

  gx = 2 * (q[1]*q[3] - q[0]*q[2]);

  gy = 2 * (q[0]*q[1] + q[2]*q[3]);

  gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];

  yaw = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1) * 180/M_PI;

  pitch = atan(gx / sqrt(gy*gy + gz*gz))  * 180/M_PI;

  roll = atan(gy / sqrt(gx*gx + gz*gz))  * 180/M_PI;

  qDebug() << "In" << __FUNCTION__ << ", yaw/pitch/roll:" << yaw << pitch << roll;

}

  double vx, vy, vz, wx, wy, wz;

  double ex, ey, ez;

  // auxiliary variables to reduce number of repeated operations

  double q0q0 = q0*q0;

  double q0q1 = q0*q1;

  double q2q3 = q2*q3;

  double q3q3 = q3*q3;          

  // normalise the measurements

  norm = sqrt(ax*ax + ay*ay + az*az);       

  ax = ax / norm;

  ay = ay / norm;

  az = az / norm;

  // Get current time in milliseconds

  QTime time = QTime::currentTime();

  now = time.hour() * 3600 * 1000 + time.minute() * 60 * 1000 + time.second() * 1000 + time.msec();

  if (lastUpdate == 0) {

  }

  // If midnight just passed by, calculate the correct halfT

	halfT = (now - lastUpdate) / 2000.0;

  }

  lastUpdate = now;

  norm = sqrt(mx*mx + my*my + mz*mz);          

  mx = mx / norm;

  my = my / norm;

  mz = mz / norm;

  // compute reference direction of flux

  hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);

  hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);

  hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         

  bx = sqrt((hx*hx) + (hy*hy));

  bz = hz;     

  // estimated direction of gravity and flux (v and w)

  vx = 2*(q1q3 - q0q2);

  wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);

  wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  

  // error is sum of cross product between reference direction of fields and direction measured by sensors

  ex = (ay*vz - az*vy) + (my*wz - mz*wy);

  ey = (az*vx - ax*vz) + (mz*wx - mx*wz);

  ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

  // integral error scaled integral gain

  exInt = exInt + ex*Ki;

  eyInt = eyInt + ey*Ki;

  ezInt = ezInt + ez*Ki;

  // adjusted gyroscope measurements

  gx = gx + Kp*ex + exInt;

  gy = gy + Kp*ey + eyInt;

  gz = gz + Kp*ez + ezInt;

  // integrate quaternion rate and normalise

  q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;

  q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;

  q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;

  q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  

  // normalise quaternion

  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

  q0 = q0 / norm;

  q1 = q1 / norm;

  q2 = q2 / norm;

  q3 = q3 / norm;

}

void IMU::getQ(double * q) {

  // gyro values are expressed in millidegrees/sec, the * M_PI/180 will convert it to radians/sec

			 accRaw[0], accRaw[1], accRaw[2],

  q[0] = q0;

  q[1] = q1;

  q[2] = q2;