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