public VectorFloat getRotated(Quaternion q) { VectorFloat r = new VectorFloat(x, y, z); r.rotate(q); return(r); }
public VectorFloat getNormalized() { VectorFloat r = new VectorFloat(x, y, z); r.normalize(); return(r); }
public void DmpGetYawPitchRoll(float[] data, Quaternion q, VectorFloat gravity) { // yaw: (about Z axis) data[0] = (float)Math.Atan2(2 * q.x * q.y - 2 * q.w * q.z, 2 * q.w * q.w + 2 * q.x * q.x - 1); // pitch: (nose up/down, about Y axis) data[1] = (float)Math.Atan(gravity.x / Math.Sqrt(gravity.y * gravity.y + gravity.z * gravity.z)); // roll: (tilt left/right, about X axis) data[2] = (float)Math.Atan(gravity.y / Math.Sqrt(gravity.x * gravity.x + gravity.z * gravity.z)); }
// uint8_t dmpGetGyroAndAccelSensor(long *data, uint8_t* packet); // uint8_t dmpGetGyroSensor(long *data, uint8_t* packet); // uint8_t dmpGetControlData(long *data, uint8_t* packet); // uint8_t dmpGetTemperature(long *data, uint8_t* packet); // uint8_t dmpGetGravity(long *data, uint8_t* packet); public VectorFloat DmpGetGravity(Quaternion q) { VectorFloat v = new VectorFloat(); v.x = 2 * (q.x * q.z - q.w * q.y); v.y = 2 * (q.w * q.x + q.y * q.z); v.z = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z; return(v); }
public VectorFloat Filter(VectorFloat v, float dt, float hz) { float alpha = FilterUtil.Alpha(hz, dt); VectorFloat last = v * alpha + current * (1.0f - alpha); current.x = last.x; current.y = last.y; current.z = last.z; return(last); }
// uint8_t dmpSetLinearAccelFilterCoefficient(float coef); // uint8_t dmpGetLinearAccel(long *data, uint8_t* packet); VectorInt16 DmpGetLinearAccel(VectorInt16 vRaw, VectorFloat gravity) { // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) VectorInt16 v = new VectorInt16(); v.x = (short)(vRaw.x - gravity.x * 4096); v.y = (short)(vRaw.y - gravity.y * 4096); v.z = (short)(vRaw.z - gravity.z * 4096); return(v); }
public VectorFloat Update(VectorFloat error, float dtime) { error_integral += error * dtime; error_derivate = (error - previous_error) / dtime; previous_error = error; error_integral.x = MathUtil.Clamp(error_integral.x, -I_max.x, I_max.x); error_integral.y = MathUtil.Clamp(error_integral.y, -I_max.y, I_max.y); error_integral.z = MathUtil.Clamp(error_integral.z, -I_max.z, I_max.z); return(error * P + error_integral * I + error_derivate * D); }
public VectorFloat Filter(VectorFloat v, float dt, float hz_x, float hz_y, float hz_z) { VectorFloat alpha = new VectorFloat(FilterUtil.Alpha(hz_x, dt), FilterUtil.Alpha(hz_y, dt), FilterUtil.Alpha(hz_z, dt)); VectorFloat last = v * alpha + current * (new VectorFloat() { x = 1, y = 1, z = 1 } -alpha); current.x = last.x; current.y = last.y; current.z = last.z; return(last); }
public VectorFloat Filter(VectorFloat v, float dt) { if (first) { first = false; last = v; } VectorFloat ret = (v - last) / dt; last = v; return(ret); }
public VectorFloat Filter(VectorFloat v, float dt, float hz_x, float hz_y, float hz_z) { if (first) { first = false; last = v; } VectorFloat d = (v - last) / dt; last = v; VectorFloat alpha = new VectorFloat(FilterUtil.Alpha(hz_x, dt), FilterUtil.Alpha(hz_y, dt), FilterUtil.Alpha(hz_z, dt)); current = alpha * (current + d); return(current); }
public void Update(float dtime) { motion = mpu.GetMotion6(); // these methods (and a few others) are also available //accelgyro.getAcceleration(&ax, &ay, &az); //accelgyro.getRotation(&gx, &gy, &gz); // display accel/gyro x/y/z values //Console.WriteLine("a/g: {0} {1} {2} {3} {4} {5}", m.ax, m.ay, m.az, m.gx, m.gy, m.gz); if (use_dmp && dmpReady) { ushort fifoCount = mpu.GetFIFOCount(); if (fifoCount == 1024) { // reset so we can continue cleanly mpu.ResetFIFO(); _logger.Debug("MPU-6050 IMU FIFO overflow"); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (fifoCount >= 42) { // read a packet from FIFO mpu.GetFIFOBytes(fifoBuffer, (byte)packetSize); q = mpu.DmpGetQuaternion(fifoBuffer); gravity = mpu.DmpGetGravity(q); mpu.DmpGetYawPitchRoll(ypr, q, gravity); //Console.WriteLine("quat {0} {1} {2} {3} ", q.w, q.x, q.y, q.z); //Console.WriteLine("ypr {0} {1} {2} ", ypr[0] * 180 / Math.PI, ypr[1] * 180 / Math.PI, ypr[2] * 180 / Math.PI); } } }
public void Calibrate() { gyro_calibration = GetUncalibratedGyro() * -1; accel_calibration = new VectorFloat(0, 0, 1) - GetUncalibratedAccel(); }
public VectorFloat Filter(VectorFloat v, float dt, float hz) { return(Filter(v, dt, hz, hz, hz)); }
public VectorFloat Filter(VectorFloat v, float dt) { integral += v * dt; return(integral); }