m5stick-imu-yaw-pitch-roll.js

m5.onconnect = async () => {
  await m5.setupIMUWait("MPU6886");

  let timeA = new Date();
  while (1) {
    let data = await m5.imu.getAllDataWait();
    let timeB = new Date();
    var msDiff = timeB.getTime() - timeA.getTime();
    timeA = timeB;
    sampleFreq = 1000 / msDiff;
    let result = MahonyAHRSupdateIMU(data);
    console.log(sampleFreq,result);
  }
};

let sampleFreq = 25.0;		// sample frequency in Hz
const twoKpDef	= (2.0 * 1.0)	// 2 * proportional gain
const twoKiDef	= (2.0 * 0.0)	// 2 * integral gain
let twoKp = twoKpDef;											// 2 * proportional gain (Kp)
let twoKi = twoKiDef;
const RAD_TO_DEG = 57.29578;
let q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0;
let integralFBx = 0.0, integralFBy = 0.0, integralFBz = 0.0;

function MahonyAHRSupdateIMU(data) {
  let { gyroscope : {x: gx , y:gy, z:gz}, accelerometer: {x: ax , y:ay, z:az}} = data;
  let pitch, roll, yaw;
  let recipNorm;
  let halfvx, halfvy, halfvz;
  let halfex, halfey, halfez;
  let qa, qb, qc;

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) {

    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // Estimated direction of gravity and vector perpendicular to magnetic flux
    halfvx = q1 * q3 - q0 * q2;
    halfvy = q0 * q1 + q2 * q3;
    halfvz = q0 * q0 - 0.5 + q3 * q3;

    // Error is sum of cross product between estimated and measured direction of gravity
    halfex = (ay * halfvz - az * halfvy);
    halfey = (az * halfvx - ax * halfvz);
    halfez = (ax * halfvy - ay * halfvx);

    // Compute and apply integral feedback if enabled
    if (twoKi > 0.0) {
      integralFBx += twoKi * halfex * (1.0 / sampleFreq);	// integral error scaled by Ki
      integralFBy += twoKi * halfey * (1.0 / sampleFreq);
      integralFBz += twoKi * halfez * (1.0 / sampleFreq);
      gx += integralFBx;	// apply integral feedback
      gy += integralFBy;
      gz += integralFBz;
    } else {
      integralFBx = 0.0;	// prevent integral windup
      integralFBy = 0.0;
      integralFBz = 0.0;
    }

    // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
  }

  // Integrate rate of change of quaternion
  gx *= (0.5 * (1.0 / sampleFreq));		// pre-multiply common factors
  gy *= (0.5 * (1.0 / sampleFreq));
  gz *= (0.5 * (1.0 / sampleFreq));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;

  pitch = Math.asin(-2 * q1 * q3 + 2 * q0 * q2);	// pitch
  roll = Math.atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1);	// roll
  yaw = Math.atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3);	//yaw

  pitch *= RAD_TO_DEG;
  yaw *= RAD_TO_DEG;
  // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is
  // 	8° 30' E  ± 0° 21' (or 8.5°) on 2016-07-19
  // - http://www.ngdc.noaa.gov/geomag-web/#declination
  yaw -= 8.5;
  roll *= RAD_TO_DEG;

  return {pitch, roll, yaw};
  ///Serial.printf("%f    %f    %f \r\n",  pitch, roll, yaw);
}

function invSqrt(x) {
  return Q_rsqrt(x);
}

function Q_rsqrt(number) {
  var i;
  var x2, y;
  const threehalfs = 1.5;

  x2 = number * 0.5;
  y = number;
  //evil floating bit level hacking
  var buf = new ArrayBuffer(4);
  (new Float32Array(buf))[0] = number;
  i = (new Uint32Array(buf))[0];
  i = (0x5f3759df - (i >> 1)); //What the fuck?
  (new Uint32Array(buf))[0] = i;
  y = (new Float32Array(buf))[0];
  y = y * (threehalfs - (x2 * y * y));   // 1st iteration
//  y  = y * ( threehalfs - ( x2 * y * y ) );   // 2nd iteration, this can be removed

  return y;
}