Beispiel #1
0
        public VectorFloat getRotated(Quaternion q)
        {
            VectorFloat r = new VectorFloat(x, y, z);

            r.rotate(q);
            return(r);
        }
Beispiel #2
0
        public VectorFloat getNormalized()
        {
            VectorFloat r = new VectorFloat(x, y, z);

            r.normalize();
            return(r);
        }
Beispiel #3
0
 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));
 }
Beispiel #4
0
        // 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);
        }
Beispiel #5
0
        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);
        }
Beispiel #6
0
        // 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);
        }
Beispiel #7
0
        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);
        }
Beispiel #8
0
        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);
        }
Beispiel #9
0
        public VectorFloat Filter(VectorFloat v, float dt)
        {
            if (first)
            {
                first = false;
                last  = v;
            }

            VectorFloat ret = (v - last) / dt;

            last = v;

            return(ret);
        }
Beispiel #10
0
        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);
        }
Beispiel #11
0
        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);
                }
            }
        }
Beispiel #12
0
 public void Calibrate()
 {
     gyro_calibration  = GetUncalibratedGyro() * -1;
     accel_calibration = new VectorFloat(0, 0, 1) - GetUncalibratedAccel();
 }
Beispiel #13
0
 public VectorFloat Filter(VectorFloat v, float dt, float hz)
 {
     return(Filter(v, dt, hz, hz, hz));
 }
Beispiel #14
0
 public VectorFloat Filter(VectorFloat v, float dt)
 {
     integral += v * dt;
     return(integral);
 }