Android

[CyberVillain]

As long as the orientation data is not used for drift-correction, it might be a good idea to also put back the support for Wireless IMU in the plugin, because it's already in the store.

The Mahony fusion uses all 3 sensors (Which includes some drift correction), doesnt matter if we use the Wireless IMU or the custom FreePIE App, the difference is that with the FreePIE app you still have the choice to output Google's fused data.

I will change the code to the deprecated way tonight. Im also oening a question on Stackoverflow

Drift is almost non-existent on the iPhone. Apple did a good job with their sensor fusion package. If not for the network stutter it would be one of the best trackers available.

We should ask Apple to release that code as open source so we could use it for any of our 9 DOF sensors. (Irony)

[CyberVillain]

I pushed the Android code to master, I've done what I can, now its up to the community to make it better

[mahler]

I pushed the Android code to master, I've done what I can, now its up to the community to make it better

I should have said this earlier, but when I opened the Android project in eclipse and deployed it to my phone.. it would crash immediately.
Don't remember exactly how I fixed it. I remember doing several things:

1) Project specific settings > use JDK 1.6 to compile
2) Change project-folder name to something without spaces (all lower case)

Not serious, but it will be just a bit easier for other developers if you could update this.

[CyberVillain]

I pushed the Android code to master, I've done what I can, now its up to the community to make it better

I should have said this earlier, but when I opened the Android project in eclipse and deployed it to my phone.. it would crash immediately.
Don't remember exactly how I fixed it. I remember doing several things:

1) Project specific settings > use JDK 1.6 to compile
2) Change project-folder name to something without spaces (all lower case)

Not serious, but it will be just a bit easier for other developers if you could update this.

Do you still have the code? Then you can push that to the master branch.

[mahler]

I should have said this earlier, but when I opened the Android project in eclipse and deployed it to my phone.. it would crash immediately. Don't remember exactly how I fixed it. I remember doing several things:

1) Project specific settings > use JDK 1.6 to compile
2) Change project-folder name to something without spaces (all lower case)

Not serious, but it will be just a bit easier for other developers if you could update this.

Do you still have the code? Then you can push that to the master branch.

I tested it again at home... and the version that you merged with the master did not have the same problem.
Now that I think about it, it must have been the /gen files which were comitted.
I did have to set my eclipse project settings, but I'm not sure I want to add the eclipse .settings folder to GitHub (kinda bad practice)
Did make some other minor changes. Created a pull-request. Just make you test it first.

[CyberVillain]

I need to add a listener to the stop event of the main activity and kill my udp thread, if you leave the FreePIE IMU screen (Back or home button) the thread wont die, but the activity will. After a while you will have lots of threads running

My computer with the my VMWare dev machine died on on me last night, so I have to get that working before I can fix this bug

[Parity]

I am using FreePIE and its android app to be able to look around ingame by using my tablets sensors.
A simple script is in action basicly doing something like

Code: Select all

mouse.DeltaX = delta_yaw * factor_x
mouse.DeltaY = delta_roll * factor_y

Basicly it works quite well, I can look around up and down and I can turn myself all around to inspect things around me.
But it only works flawless in the "lower hemisphere" and the "upper hemisphere". It seems to be quite glitchy when the tablet is pointed directly to the horizon. Yaw values will change quite fast in that area.

Is there some transformation needed from roll, yaw and pitch to get azimuth and polar angle independently?

Parity

PS: I get values only from my tablet running Android 4.1. When using my phone, running 2.3.3 I do receive values, but they are all zero. Maybe one can add the display of roll, pitch and yaw to the app?

[CyberVillain]

I am using FreePIE and its android app to be able to look around ingame by using my tablets sensors.
A simple script is in action basicly doing something like

Code: Select all

mouse.DeltaX = delta_yaw * factor_x
mouse.DeltaY = delta_roll * factor_y

Basicly it works quite well, I can look around up and down and I can turn myself all around to inspect things around me.
But it only works flawless in the "lower hemisphere" and the "upper hemisphere". It seems to be quite glitchy when the tablet is pointed directly to the horizon. Yaw values will change quite fast in that area.

Sounds like drift, the google sensor fusion has less drift than the custom fusion that I use, but its more laggy. Try both, I'm also thinking of trying to port the AHRS fusion code from Arduino to C#, maybe that will give some less drift.

Is there some transformation needed from roll, yaw and pitch to get azimuth and polar angle independently?

Google fusion uses azimuth and the Custom one uses contiues yaw.

PS: I get values only from my tablet running Android 4.1. When using my phone, running 2.3.3 I do receive values, but they are all zero. Maybe one can add the display of roll, pitch and yaw to the app?

Sounds like a network problem, the plugin will always give you zero even when not connected.Yeah, i can display the values in the app

[Parity]

Thank your for your reply.

Sounds like drift, the google sensor fusion has less drift than the custom fusion that I use, but its more laggy. Try both, I'm also thinking of trying to port the AHRS fusion code from Arduino to C#, maybe that will give some less drift.

Hm, doesn't seem to be a drift-issue. It is quite reproducable if I repeat the same movement. I will try to compare if sensors in my phone will behave the same. Maybe I should create a small video do demonstrate that.
Unfortunatly FreePIE (while basically doing a perfect job for what I try to do) keeps chrashing every minute or something, so testing is a bit annoying ("Unhandled Exception Occured").

Sounds like a network problem, the plugin will always give you zero even when not connected.Yeah, i can display the values in the app

I don't think so. I do get values from the phone when using SQUIDY and the Android2 plugin for FreePIE.
Also it does not only keep displaying 0, but really overrides the values with 0 every few milliseconds.

[mahler]

Thank your for your reply.
[...]
I don't think so. I do get values from the phone when using SQUIDY and the Android2 plugin for FreePIE.
Also it does not only keep displaying 0, but really overrides the values with 0 every few milliseconds.

Android2? That Plugin should not be there.
Did you take the FreePIE code and Android APK from here?
https://github.com/AndersMalmgren/FreePIE

Attachements in this thread are probably outdated.

[Parity]

I use the FreePIE code and APK from github. Plus the Android2 from mahler's post on the first page of this thread, to be able to use SQUIDY.
Both plugins are working fine together without interfering.

As stated: Only SQUIDY with Android2 gives me the sensordata from my smartphone running Android 2.3.3
My tablet running Android 4.1 works fine with FreePIE APK and "android" plugin, will test SQUIDY on tablet right now...

edit:
On the tablet both ways are working fine. Even simultaniously...
While Android2.Yaw reaches from 0..1 and android.GoogleYaw reaches 0..360 the following two lines give me the same value:

Code: Select all

diagnostics:watch(360*Android2.Yaw)
diagnostics:watch(android.GoogleYaw)

[CyberVillain]

Will get back to you on that Parity, have to investigate.

On a nother note

Spent the evning converting Razor AHRS to C# code, all I got Is NaN :/
If you guys are good at IMU's and C please take a look, Its based on the Razor AHRS code for Arduino

I take the accel values directly off the udp, teh AHRS software does this, i dont know if that could be the proble

Code: Select all

    // No multiply by -1 for coordinate system transformation here, because of double negation:
    // We want the gravity vector, which is negated acceleration vector.
    accel[0] = (((int) buff[3]) << 8) | buff[2];  // X axis (internal sensor y axis)
    accel[1] = (((int) buff[1]) << 8) | buff[0];  // Y axis (internal sensor x axis)
    accel[2] = (((int) buff[5]) << 8) | buff[4];  // Z axis (internal sensor z axis)

The magnometer read is doing alot off crap im not doing in my port

Code: Select all

// 9DOF Razor IMU SEN-10125 using HMC5843 magnetometer
#if HW__VERSION_CODE == 10125
    // MSB byte first, then LSB; X, Y, Z
    magnetom[0] = -1 * ((((int) buff[2]) << 8) | buff[3]);  // X axis (internal sensor -y axis)
    magnetom[1] = -1 * ((((int) buff[0]) << 8) | buff[1]);  // Y axis (internal sensor -x axis)
    magnetom[2] = -1 * ((((int) buff[4]) << 8) | buff[5]);  // Z axis (internal sensor -z axis)
// 9DOF Razor IMU SEN-10736 using HMC5883L magnetometer
#elif HW__VERSION_CODE == 10736
    // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
    magnetom[0] = -1 * ((((int) buff[4]) << 8) | buff[5]);  // X axis (internal sensor -y axis)
    magnetom[1] = -1 * ((((int) buff[0]) << 8) | buff[1]);  // Y axis (internal sensor -x axis)
    magnetom[2] = -1 * ((((int) buff[2]) << 8) | buff[3]);  // Z axis (internal sensor -z axis)
// 9DOF Sensor Stick SEN-10183 and SEN-10321 using HMC5843 magnetometer
#elif (HW__VERSION_CODE == 10183) || (HW__VERSION_CODE == 10321)
    // MSB byte first, then LSB; X, Y, Z
    magnetom[0] = (((int) buff[0]) << 8) | buff[1];         // X axis (internal sensor x axis)
    magnetom[1] = -1 * ((((int) buff[2]) << 8) | buff[3]);  // Y axis (internal sensor -y axis)
    magnetom[2] = -1 * ((((int) buff[4]) << 8) | buff[5]);  // Z axis (internal sensor -z axis)
// 9DOF Sensor Stick SEN-10724 using HMC5883L magnetometer
#elif HW__VERSION_CODE == 10724
    // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
    magnetom[0] = (((int) buff[0]) << 8) | buff[1];         // X axis (internal sensor x axis)
    magnetom[1] = -1 * ((((int) buff[4]) << 8) | buff[5]);  // Y axis (internal sensor -y axis)
    magnetom[2] = -1 * ((((int) buff[2]) << 8) | buff[3]);  // Z axis (internal sensor -z axis)
#endif

And the gyro

Code: Select all

    gyro[0] = -1 * ((((int) buff[2]) << 8) | buff[3]);    // X axis (internal sensor -y axis)
    gyro[1] = -1 * ((((int) buff[0]) << 8) | buff[1]);    // Y axis (internal sensor -x axis)
    gyro[2] = -1 * ((((int) buff[4]) << 8) | buff[5]);    // Z axis (internal sensor -z axis)

Ported C# code

Code: Select all

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace FreePIE.Core.Plugins.SensorFusion
{
    public static class Vector
    {
        public static float DotProduct(float[] vector1, float[] vector2)
        {
            float op = 0;

            for (int c = 0; c < 3; c++)
            {
                op += vector1[c]*vector2[c];
            }

            return op;
        }

        // Multiply the vector by a scalar. 
        public static void Scale(float[] vectorOut, float[] vectorIn, float scale2)
        {
            for (int c = 0; c < 3; c++)
            {
                vectorOut[c] = vectorIn[c]*scale2;
            }
        }

        public static void Add(float[] vectorOut, float[] vectorIn1, float[] vectorIn2)
        {
            for (int c = 0; c < 3; c++)
            {
                vectorOut[c] = vectorIn1[c] + vectorIn2[c];
            }
        }

        //Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). 
        public static void MatrixMultiply(float[][] a, float[][] b, float[][] mat)
        {
            var op = new float[3];
            for (int x = 0; x < 3; x++)
            {
                for (int y = 0; y < 3; y++)
                {
                    for (int w = 0; w < 3; w++)
                    {
                        op[w] = a[x][w]*b[w][y];
                    }
                    mat[x][y] = 0;
                    mat[x][y] = op[0] + op[1] + op[2];

                    float test = mat[x][y];
                }
            }
        }

        // Computes the cross product of two vectors
        public static void CrossProduct(float[] vectorOut, float[] v1, float[] v2)
        {
            vectorOut[0] = (v1[1]*v2[2]) - (v1[2]*v2[1]);
            vectorOut[1] = (v1[2]*v2[0]) - (v1[0]*v2[2]);
            vectorOut[2] = (v1[0]*v2[1]) - (v1[1]*v2[0]);
        }

        public static float Constrain(float x, float a, float b)
        {
            if (x < a)
            {
                return a;
            }
            else if (b < x)
            {
                return b;
            }
            else
                return x;
        }
    }

    public class RazorAhrs
    {
        private float G_Dt; // Integration time for DCM algorithm

        private float[] accel = new float[3];
        private float[] magnetom = new float[3];
        private float[] gyro = new float[3];

        // Euler angles
        private float yaw;
        private float pitch;
        private float roll;

        // DCM variables
        private float MAG_Heading;
        private float[] Accel_Vector = new float[] {0, 0, 0}; // Store the acceleration in a vector
        private float[] Gyro_Vector = new float[] {0, 0, 0}; // Store the gyros turn rate in a vector
        private float[] Omega_Vector = new float[] {0, 0, 0}; // Corrected Gyro_Vector data
        private float[] Omega_P = new float[] {0, 0, 0}; // Omega Proportional correction
        private float[] Omega_I = new float[] {0, 0, 0}; // Omega Integrator
        private float[] Omega = new float[] {0, 0, 0};
        private float[] errorRollPitch = new float[] {0, 0, 0};
        private float[] errorYaw = new float[] {0, 0, 0};
        private float[][] DCM_Matrix = new[] { new float[] { 1, 0, 0 }, new float[] { 0, 1, 0 }, new float[] { 0, 0, 1 } };
        private float[][] Update_Matrix = new[] { new float[] { 0, 1, 2 }, new float[] { 3, 4, 5 }, new float[] { 6, 7, 8 } };
        private float[][] Temporary_Matrix = new[] { new float[] { 0, 0, 0 }, new float[] { 0, 0, 0 }, new float[] { 0, 0, 0 } };

        // DCM parameters
        private const float Kp_ROLLPITCH = 0.02f;
        private const float Ki_ROLLPITCH = 0.00002f;
        private const float Kp_YAW = 1.2f;
        private const float Ki_YAW = 0.00002f;

        private bool calibrated;
        private const float GRAVITY = 256.0f; // "1G reference" used for DCM filter and accelerometer calibration

        // Gain for gyroscope (ITG-3200)
        private const float GYRO_GAIN = 0.06957f; // Same gain on all axes

        // SENSOR CALIBRATION
        /*****************************************************************/
        // How to calibrate? Read the tutorial at http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs
        // Put MIN/MAX and OFFSET readings for your board here!
        // Accelerometer
        // "accel x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"

        private float ACCEL_X_MIN = -278f;
        private float ACCEL_X_MAX = 258f;
        private float ACCEL_Y_MIN = -260f;
        private float ACCEL_Y_MAX = 292f;
        private float ACCEL_Z_MIN = -282f;
        private float ACCEL_Z_MAX = 250f;

        // Magnetometer
        // "magn x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
        private float MAGN_X_MIN = -600f;
        private float MAGN_X_MAX = 600f;
        private float MAGN_Y_MIN = 600f;
        private float MAGN_Y_MAX = 600f;
        private float MAGN_Z_MIN = -600f;
        private float MAGN_Z_MAX = 600f;

        // Gyroscope
        // "gyro x,y,z (current/average) = .../OFFSET_X  .../OFFSET_Y  .../OFFSET_Z
        private float GYRO_AVERAGE_OFFSET_X = -22.13f;
        private float GYRO_AVERAGE_OFFSET_Y = 22.31f;
        private float GYRO_AVERAGE_OFFSET_Z = 6.88f;


        // Sensor calibration scale and offset values
        private float ACCEL_X_OFFSET;
        private float ACCEL_Y_OFFSET;
        private float ACCEL_Z_OFFSET;
        private float ACCEL_X_SCALE;
        private float ACCEL_Y_SCALE;
        private float ACCEL_Z_SCALE;

        private float MAGN_X_OFFSET;
        private float MAGN_Y_OFFSET;
        private float MAGN_Z_OFFSET;
        private float MAGN_X_SCALE;
        private float MAGN_Y_SCALE;
        private float MAGN_Z_SCALE;

        private DateTime timestamp = DateTime.Now;

        public void Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
        {
            gyro[0] = gx;
            gyro[1] = gy;
            gyro[2] = gz;

            accel[0] = ax;
            accel[1] = ay;
            accel[2] = az;

            magnetom[0] = mx;
            magnetom[1] = my;
            magnetom[2] = mz;

            G_Dt = (float)(DateTime.Now - timestamp).TotalSeconds; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)

            timestamp = DateTime.Now;


            // Apply sensor calibration
            CompensateSensorErrors();

            // Run DCM algorithm
            CompassHeading(); // Calculate magnetic heading
            MatrixUpdate();
            Normalize();
            Drift_correction();
            Euler_angles();
        }

        public float Yaw
        {
            get { return yaw; }
        }

        // Apply calibration to raw sensor readings
        private void CompensateSensorErrors()
        {
            if (!calibrated)
            {
                // Sensor calibration scale and offset values
                ACCEL_X_OFFSET = ((ACCEL_X_MIN + ACCEL_X_MAX)/2.0f);
                ACCEL_Y_OFFSET = ((ACCEL_Y_MIN + ACCEL_Y_MAX)/2.0f);
                ACCEL_Z_OFFSET = ((ACCEL_Z_MIN + ACCEL_Z_MAX)/2.0f);
                ACCEL_X_SCALE = (GRAVITY/(ACCEL_X_MAX - ACCEL_X_OFFSET));
                ACCEL_Y_SCALE = (GRAVITY/(ACCEL_Y_MAX - ACCEL_Y_OFFSET));
                ACCEL_Z_SCALE = (GRAVITY/(ACCEL_Z_MAX - ACCEL_Z_OFFSET));

                MAGN_X_OFFSET = ((MAGN_X_MIN + MAGN_X_MAX)/2.0f);
                MAGN_Y_OFFSET = ((MAGN_Y_MIN + MAGN_Y_MAX)/2.0f);
                MAGN_Z_OFFSET = ((MAGN_Z_MIN + MAGN_Z_MAX)/2.0f);
                MAGN_X_SCALE = (100.0f/(MAGN_X_MAX - MAGN_X_OFFSET));
                MAGN_Y_SCALE = (100.0f/(MAGN_Y_MAX - MAGN_Y_OFFSET));
                MAGN_Z_SCALE = (100.0f/(MAGN_Z_MAX - MAGN_Z_OFFSET));
                calibrated = true;
            }

            // Compensate accelerometer error
            accel[0] = (accel[0] - ACCEL_X_OFFSET)*ACCEL_X_SCALE;
            accel[1] = (accel[1] - ACCEL_Y_OFFSET)*ACCEL_Y_SCALE;
            accel[2] = (accel[2] - ACCEL_Z_OFFSET)*ACCEL_Z_SCALE;

            // Compensate magnetometer error
            magnetom[0] = (magnetom[0] - MAGN_X_OFFSET)*MAGN_X_SCALE;
            magnetom[1] = (magnetom[1] - MAGN_Y_OFFSET)*MAGN_Y_SCALE;
            magnetom[2] = (magnetom[2] - MAGN_Z_OFFSET)*MAGN_Z_SCALE;

            // Compensate gyroscope error
            gyro[0] -= GYRO_AVERAGE_OFFSET_X;
            gyro[1] -= GYRO_AVERAGE_OFFSET_Y;
            gyro[2] -= GYRO_AVERAGE_OFFSET_Z;
        }

        private void CompassHeading()
        {
            float mag_x;
            float mag_y;
            float cos_roll;
            float sin_roll;
            float cos_pitch;
            float sin_pitch;

            cos_roll = (float) Math.Cos(roll);
            sin_roll = (float) Math.Sin(roll);
            cos_pitch = (float) Math.Cos(pitch);
            sin_pitch = (float) Math.Sin(pitch);

            // Tilt compensated magnetic field X
            mag_x = magnetom[0]*cos_pitch + magnetom[1]*sin_roll*sin_pitch + magnetom[2]*cos_roll*sin_pitch;
            // Tilt compensated magnetic field Y
            mag_y = magnetom[1]*cos_roll - magnetom[2]*sin_roll;
            // Magnetic Heading
            MAG_Heading = (float) Math.Atan2(-mag_y, mag_x);
        }

        private float GetGyroScaledRad(float x)
        {
            return x*((float) Math.PI/180f)*GYRO_GAIN; // Calculate the scaled gyro readings in radians per second
        }

        private void MatrixUpdate()
        {

            Gyro_Vector[0] = GetGyroScaledRad(gyro[0]); //gyro x roll
            Gyro_Vector[1] = GetGyroScaledRad(gyro[1]); //gyro y pitch
            Gyro_Vector[2] = GetGyroScaledRad(gyro[2]); //gyro z yaw

            Accel_Vector[0] = accel[0];
            Accel_Vector[1] = accel[1];
            Accel_Vector[2] = accel[2];

            Vector.Add(Omega, Gyro_Vector, Omega_I); //adding proportional term
            Vector.Add(Omega_Vector, Omega, Omega_P); //adding Integrator term

            Update_Matrix[0][0] = 0;
            Update_Matrix[0][1] = -G_Dt*Omega_Vector[2]; //-z
            Update_Matrix[0][2] = G_Dt*Omega_Vector[1]; //y
            Update_Matrix[1][0] = G_Dt*Omega_Vector[2]; //z
            Update_Matrix[1][1] = 0;
            Update_Matrix[1][2] = -G_Dt*Omega_Vector[0]; //-x
            Update_Matrix[2][0] = -G_Dt*Omega_Vector[1]; //-y
            Update_Matrix[2][1] = G_Dt*Omega_Vector[0]; //x
            Update_Matrix[2][2] = 0;

            Vector.MatrixMultiply(DCM_Matrix, Update_Matrix, Temporary_Matrix); //a*b=c

            for (int x = 0; x < 3; x++) //Matrix Addition (update)
            {
                for (int y = 0; y < 3; y++)
                {
                    DCM_Matrix[x][y] += Temporary_Matrix[x][y];
                }
            }
        }

        private void Normalize()
        {
            float error = 0;
            var temporary = new[] {new float[] {0, 0, 0}, new float[] {0, 0, 0}, new float[] {0, 0, 0}};
            float renorm = 0;

            error = -Vector.DotProduct(DCM_Matrix[0], DCM_Matrix[1])*.5f; //eq.19

            Vector.Scale(temporary[0], DCM_Matrix[1], error); //eq.19
            Vector.Scale(temporary[1], DCM_Matrix[0], error); //eq.19

            Vector.Scale(temporary[0], temporary[0], DCM_Matrix[0][0]); //eq.19
            Vector.Scale(temporary[1], temporary[1], DCM_Matrix[1][0]); //eq.19

            Vector.CrossProduct(temporary[2], temporary[0], temporary[1]); // c= a x b //eq.20

            renorm = .5f*(3 - Vector.DotProduct(temporary[0], temporary[0])); //eq.21
            Vector.Scale(DCM_Matrix[0], temporary[0], renorm);

            renorm = .5f*(3 - Vector.DotProduct(temporary[1], temporary[1])); //eq.21
            Vector.Scale(DCM_Matrix[1], temporary[1], renorm);

            renorm = .5f*(3 - Vector.DotProduct(temporary[2], temporary[2])); //eq.21
            Vector.Scale(DCM_Matrix[2], temporary[2], renorm);
        }

        private void Drift_correction()
        {
            float mag_heading_x;
            float mag_heading_y;
            float errorCourse;
            //Compensation the Roll, Pitch and Yaw drift. 
            var Scaled_Omega_P = new float[3];
            var Scaled_Omega_I = new float[3];
            float Accel_magnitude;
            float Accel_weight;


            //*****Roll and Pitch***************

            // Calculate the magnitude of the accelerometer vector
            Accel_magnitude =
                (float)
                Math.Sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] +
                          Accel_Vector[2]*Accel_Vector[2]);
            Accel_magnitude = Accel_magnitude/GRAVITY; // Scale to gravity.
            // Dynamic weighting of accelerometer info (reliability filter)
            // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
            Accel_weight = Vector.Constrain(1 - 2*Math.Abs(1 - Accel_magnitude), 0, 1); //  

            Vector.CrossProduct(errorRollPitch, Accel_Vector, DCM_Matrix[2]); //adjust the ground of reference
            Vector.Scale(Omega_P, errorRollPitch, Kp_ROLLPITCH*Accel_weight);

            Vector.Scale(Scaled_Omega_I, errorRollPitch, Ki_ROLLPITCH*Accel_weight);
            Vector.Add(Omega_I, Omega_I, Scaled_Omega_I);

            //*****YAW***************
            // We make the gyro YAW drift correction based on compass magnetic heading

            mag_heading_x = (float) Math.Cos(MAG_Heading);
            mag_heading_y = (float) Math.Sin(MAG_Heading);
            errorCourse = (DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
            Vector.Scale(errorYaw, DCM_Matrix[2], errorCourse);
                //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.

            Vector.Scale(Scaled_Omega_P, errorYaw, Kp_YAW); //.01proportional of YAW.
            Vector.Add(Omega_P, Omega_P, Scaled_Omega_P); //Adding  Proportional.

            Vector.Scale(Scaled_Omega_I, errorYaw, Ki_YAW); //.00001Integrator
            Vector.Add(Omega_I, Omega_I, Scaled_Omega_I); //adding integrator to the Omega_I
        }

        private void Euler_angles()
        {
            pitch = (float) -Math.Asin(DCM_Matrix[2][0]);
            roll = (float) Math.Atan2(DCM_Matrix[2][1], DCM_Matrix[2][2]);
            yaw = (float) Math.Atan2(DCM_Matrix[1][0], DCM_Matrix[0][0]);
        }
    }
}

[CyberVillain]

Hmm, sorry the only thing I have noticed is drift (Quite a huge amount of drift with my Phone). Is it the same both with the FreePIE fusiin (Mahony fusion) and Google onboard fusion?

Strange with the error, the Android plugin is all managed code and all managed code loops should be proctected, it looks like you have access to VS? Could you please debug the app while testing the Android plugin and let me know where it crashes?

Strange that it works with Squidy and not the FreePIE app, I will add the sensor data for debugging as soon as possible

Thank your for your reply.

Sounds like drift, the google sensor fusion has less drift than the custom fusion that I use, but its more laggy. Try both, I'm also thinking of trying to port the AHRS fusion code from Arduino to C#, maybe that will give some less drift.

Hm, doesn't seem to be a drift-issue. It is quite reproducable if I repeat the same movement. I will try to compare if sensors in my phone will behave the same. Maybe I should create a small video do demonstrate that.
Unfortunatly FreePIE (while basically doing a perfect job for what I try to do) keeps chrashing every minute or something, so testing is a bit annoying ("Unhandled Exception Occured").

Sounds like a network problem, the plugin will always give you zero even when not connected.Yeah, i can display the values in the app

I don't think so. I do get values from the phone when using SQUIDY and the Android2 plugin for FreePIE.
Also it does not only keep displaying 0, but really overrides the values with 0 every few milliseconds.

[Parity]

I tried to debug the application, and that apparently fixed the crashes of FreePIE for me.
I was previously using a compiled version on "computer B" (Win7,x64) that I compiled on "computer A" (WinXP,x86) with VS2010. After running FreePIE in VS2010 on "computer B" I do not see any crashes anymore.

So the crashes are gone, I get values from my tablet (SQUIDY and FreePIE-APK) and from my phone (only SQUIDY).
Now I will have to look into, why the orientation values do not behave the way I expect them to. When the display surface is perpendicular to the ground, the yaw value does strange things...

I will report back if I understand that.

[CyberVillain]

I tried to debug the application, and that apparently fixed the crashes of FreePIE for me.
I was previously using a compiled version on "computer B" (Win7,x64) that I compiled on "computer A" (WinXP,x86) with VS2010. After running FreePIE in VS2010 on "computer B" I do not see any crashes anymore.

So the crashes are gone, I get values from my tablet (SQUIDY and FreePIE-APK) and from my phone (only SQUIDY).
Now I will have to look into, why the orientation values do not behave the way I expect them to. When the display surface is perpendicular to the ground, the yaw value does strange things...

I will report back if I understand that.

Strange that it does not work when compiled on the XP machine, shouldn't matter. Yeah, please try to see if you can figure out whats causing it to behave like that, my game/dev machine broke down on me so I'm building a new one, wont have time to look at getting debug values into the Android app until earliest this weekend.

Also, please put a breakpoint at row 105 and 109 in FreePIE.Core.Plugins.AndroidPlugin and connect to the PC with the FreePIE android app
If it stops on row 105 you have some kind of network problem, if it does not stop on row 109 it means that your phones UDP packets does not get through to the plugin. If it does go through please look at the raw values and see if it looks correct, and then look at the mahony fusion why it doesnt work.

[Parity]

It DOES stop on line 109.

Code: Select all

var flag = bytes[0];

Values received are zero for: gx, gy, gz, mx, my, mz
as well as zeros for GoogleYaw, GooglePitch and GoogleRoll.

ax, ay and az contains values that look fine.
Maybe the phone does lack some sensors that the Tables does have. But on the other hand, when using SQUIDY and Android2.Pitch, *.Roll, *.Yaw the values are valid and work fine.

I am just playing around with it on my phone. You do not need to put very much effort in it to make it working, at least for me. I just tried to use the phone to verify the values coming from the tablet. But I think the behaviour I see there is sensor-related.

[CyberVillain]

Your phone probably only has accelerometer then :/
But if you check Send orientation in the Android App (ANd use GoogleYaw, etc in FreePIE) it should give you the same result as Squidy since they both use the built in orientation

[Parity]

Thank you for the support.
Works now. Things I learned now:

a) Changes in the checkboxes of the APK apply only, if I stop and restart the transmission from the APK (ON/OFF Button), changes while sending do not apply. That's why I didn't find out about b) earlier.

b) I usually had both options on: "send orientation" and "send raw data" *.GoolgeYaw and so on do only work if I check "send orientation" exclusively. If I check both boxes values will become zero (on my phone).

[mahler]

I am just playing around with it on my phone. You do not need to put very much effort in it to make it working, at least for me. I just tried to use the phone to verify the values coming from the tablet. But I think the behaviour I see there is sensor-related.

To be sure about your phone senor data, you can download Wireless IMU (or any other app which can display ACC/GYRO/MAG values..)

But if you check Send orientation in the Android App (ANd use GoogleYaw, etc in FreePIE) it should give you the same result as Squidy since they both use the built in orientation

A few days back, the creator of SquidyClient promised to send me the code, but he's a little busy right now. This might give us some insight in the similarity between the two.

[CyberVillain]

Thank you for the support.
Works now. Things I learned now:

a) Changes in the checkboxes of the APK apply only, if I stop and restart the transmission from the APK (ON/OFF Button), changes while sending do not apply. That's why I didn't find out about b) earlier.

b) I usually had both options on: "send orientation" and "send raw data" *.GoolgeYaw and so on do only work if I check "send orientation" exclusively. If I check both boxes values will become zero (on my phone).

a) Yeah I should probably disable the boxes so you dont get fooled that you can change them during runtime
b) Strange, for me it works with both boxes checked (And thats how I designed it to work). WHen both boxes are checked line the flag byte should be 3 (1 OR 2 bitwise) and also theck the two bools that they are set to true

Code: Select all

                var flag = bytes[0]; //Should be 3
                var sendRaw = (flag & 1) == 1; //Should be true
                var sendOrientation = (flag & 2) == 2; //Should be true

[Parity]

On Plugin-side it seems okay, I do indeed receive values. But they are zero.
I expect it to be something in the APK.

[CyberVillain]

Debugging added to APP, i've updated the APK so it should just be a matter of installing that to the phone

edit: If you dont want to kill a older phone with UI overload set the update freq to UI when debugging.

[Parity]

Updated the APK. But am I supposed to see values there?
(Screenshot from phone in attachement, will try tablet later.)

[CyberVillain]

Lol, i thought that it would let you scroll if the screen res was too low.. Thanks for testing

Yeah when running you should see values there (Due to your low res Magnetometer and Orientation is out of screen)

[mahler]

Found a possible way to speed up the AHRS code
Not sure if this of actual value, might be worth checking out.

In the MahoneyAHRS and Quaternion code from FreePIE, taken from x-io.co.uk, you will find a Math.sqrt() operation.
This is then used to divide the norm or quaternions with the result.

I read an article a few months ago about the marvelous Fast invSqrt() function invented a few decades ago.
Then today I found the same method used in the source-code for the AdjacentReality-tracker which is used in the Rift.

https://github.com/AdjacentReality/lufa ... HRS.c#L142

I haven't had time for coding in a while because I started a new job
So it's just an idea and if we're lucky it might get rid of some lag.

Edit:

The reason is probably because this code is only for C which was updated with the following comment:

Code: Select all

// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load

It might still work in C#, see: http://stackoverflow.com/questions/2688 ... ction-in-c
But according to this, there is not a big win in performance.

[CyberVillain]

The Arduino is a 8 bit CPU i can bet that its slower on calculating the Square root than C#'s Math.Sqrt on a modern PC.
They are doing something else than just a Quaternion like we do in FreePIE? Can you hook me up with the code?

If you want and get time please look at the Razor AHRS C# on previous page and see if you can find whats wrong, I think the sensor values on the Arduino boards are different than Android and thats why it fails.

Found a possible way to speed up the AHRS code
Not sure if this of actual value, might be worth checking out.

In the MahoneyAHRS and Quaternion code from FreePIE, taken from x-io.co.uk, you will find a Math.sqrt() operation.
This is then used to divide the norm or quaternions with the result.

I read an article a few months ago about the marvelous Fast invSqrt() function invented a few decades ago.
Then today I found the same method used in the source-code for the AdjacentReality-tracker which is used in the Rift.

https://github.com/AdjacentReality/lufa ... HRS.c#L142

I haven't had time for coding in a while because I started a new job
So it's just an idea and if we're lucky it might get rid of some lag.

Edit:

The reason is probably because this code is only for C which was updated with the following comment:

Code: Select all

// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load

It might still work in C#, see: http://stackoverflow.com/questions/2688 ... ction-in-c
But according to this, there is not a big win in performance.

[mahler]

The Arduino is a 8 bit CPU i can bet that its slower on calculating the Square root than C#'s Math.Sqrt on a modern PC.

Yes that's probably it. Still have a look at the StackOverflow post. Math.sqrt(x) is still significantly slower in C# compared to 1 / Math.sqrt(x) ... so it might be a compiler optimization. I don't know enough about compilers to know if the Mahoney/MadgwickAHRS code for C# can be optimized like that, if there isn't an explicit 1/Math.sqrt(x) division on one line.

Then again. He made 10.000.000 calculations, which took 1.7 sec. Such numbers might matter for game-engines. This may not be that interesting for a plugin which processes 'only' 300 samples/sec.

They are doing something else than just a Quaternion like we do in FreePIE? Can you hook me up with the code?

What do you mean? The code from AdjacentReality is also taken from the x-io.co.uk guys, but it's the C code, which was maintained until 29-Feb-2012 and had some improvements.

If you want and get time please look at the Razor AHRS C# on previous page and see if you can find whats wrong, I think the sensor values on the Arduino boards are different than Android and thats why it fails.

I want to, but I don't think it will be soon. Maybe this weekend.
I tried to install FreeTrack, but somehow couldn't get it to work, so all ingame testing was done with mouseX/Y-translation.
Could you provide the mouse/freetrack LUA scripts you use?

[CyberVillain]

What do you mean? The code from AdjacentReality is also taken from the x-io.co.uk guys, but it's the C code, which was maintained until 29-Feb-2012 and had some improvements.

You said...

This is then used to divide the norm or quaternions with the result.

We are doing this already no?

I want to, but I don't think it will be soon. Maybe this weekend.
I tried to install FreeTrack, but somehow couldn't get it to work, so all ingame testing was done with mouseX/Y-translation.
Could you provide the mouse/freetrack LUA scripts you use?

For Freetrack > Mouse i use
http://www.mtbs3d.com/phpBB/viewtopic.p ... 672#p74672

[mahler]

We are doing this already no?

Yes. I was just describing it. The C# code does 1.0f / Math.sqrt(x) and then multiplies with the result, effectively dividing by the sqrt. Tried to make a point that C code does the invSqrt() directly, but I realize now that it may not be worth the effort in C#.

Also ... I understand multiplication is faster than division, but this code doesn't make much sense to me:

Code: Select all

	// Integrate rate of change of quaternion to yield quaternion
	q0 += qDot1 * (1.0f / sampleFreq); // first dividing, then mutiplying?
	q1 += qDot2 * (1.0f / sampleFreq); // first dividing, then mutiplying?
	q2 += qDot3 * (1.0f / sampleFreq); // first dividing, then mutiplying?
	q3 += qDot4 * (1.0f / sampleFreq); // first dividing, then mutiplying?

For Freetrack > Mouse i use
http://www.mtbs3d.com/phpBB/viewtopic.p ... 672#p74672

Ok, I tried that one, but couldn't get the FreeTrack calibration software respond to my input. I'll have another look at it.

[CyberVillain]

Ok, I tried that one, but couldn't get the FreeTrack calibration software respond to my input. I'll have another look at it.

Ah so its a Freetrack problem not FreePIE? Have you changed the point model to the one you're using? I missed that the last time and you get really strange results. Does the points show up in the camera view in Freetrack? Have you used a normal light filter to block out everything except the points? Etc.. Freetrack works very well once configured. I use he PS3 Eye camera, very fast and repsonsive

[CyberVillain]

Added scroll view to Activity for low res. devices

[Parity]

Hey. It's me again. Changes working fine.

FreePIE with Android is totally working like I expect it to. With tablet and phone. Great work!

But while I receive excatly the values the sensors are generating, I still not happy with the results I achive.
I am quite busy these days, so it took me some time so prepare a video to explain my sensor problems I mentioned earlier.

Maybe someone can take two minutes time to view the vid.

Issue affects all axes, taking yaw values as example:
1) Yaw sensor is working fine. Values as expected when lying flat.
2) When putting tablet perpendicular to surface, values change very sensitive.
There seems to be a difference when display is facing up or facing down.
3) Looking around in VR will work, but issues with certain orientations.
4) But hardware is capable of the correct movement. there exist applications that use the sensors correctly. Last part of my video clearly shows that.

I expect that I have to do some fancy transformation of the rotation values. Any ideas where to start?

[youtube]http://www.youtube.com/watch?v=IVgHOjOTrIM[/youtube]

[CyberVillain]

Hey. It's me again. Changes working fine.

FreePIE with Android is totally working like I expect it to. With tablet and phone. Great work!

But while I receive excatly the values the sensors are generating, I still not happy with the results I achive.
I am quite busy these days, so it took me some time so prepare a video to explain my sensor problems I mentioned earlier.

Maybe someone can take two minutes time to view the vid.

Issue affects all axes, taking yaw values as example:
1) Yaw sensor is working fine. Values as expected when lying flat.
2) When putting tablet perpendicular to surface, values change very sensitive.
There seems to be a difference when display is facing up or facing down.
3) Looking around in VR will work, but issues with certain orientations.
4) But hardware is capable of the correct movement. there exist applications that use the sensors correctly. Last part of my video clearly shows that.

I expect that I have to do some fancy transformation of the rotation values. Any ideas where to start?

[youtube]http://www.youtube.com/watch?v=IVgHOjOTrIM[/youtube]

Thanks for input, good that the communication part works good now and that you retrive values boith on phone and tablet, what was the problem with your phone? Do you notice the same strange behavior with the FreePIE fusion?
The orientation parameters are fetched directly of the API we do not post processing them. There are two ways of getting orientation of from the API, for both me and Mahler the preferred way didnt give good results. So we choosed the deprecated way of retrieving orientation. I can compile a new tryout version with the other fusion for you.

I created this question about the two ways of getting IMU data of the API but didnt get any answers
http://stackoverflow.com/questions/1259 ... he-new-way


edit: Makes you wonder what fusion the app on the end uses

[Parity]

See my last post on page 4 to see what helped my phone.
This still applies like that.

Maybe it would help if some people can confirm/falsify that behaviour I am experiencing?
If you place the display vertically and move it in that way?
There are quite some apps that sucsessfully use the orientation sensors for augmented reality, so I do not belive in a defective set of sensors...

Parity

[CyberVillain]

Hmm yaw is no longer yaw when holding it like that, I think you will have to compensate for that in your FreePIE script.
I get the same behaviour like you but its not a bug its just the way the orientation of the 3 axis are defined. That game must do alot of post process to get that result.

Try this version, I get alot of strange noise when using this method, whats new is Orientation, raw values are retrived using the same method as before, However now I null the 3 sensor data each time and wait until all 3 are refilled until seneding raw data (so 300 hz should be close to 100 hz now) this could change the bahaviour of the custom fusion in FreePIE. Also note that the custom fusion wont work with your phone due to the lack of missing gyroscope.

[Parity]

Nice fast response.
And here comes my feedback:
- Jittery values, I seem to get the same noise as you do.
- But: Behaviour I explained in the video is gone. (As far as I can tell with noise.) Looks good.

As the range of the values changed like (0..360) is now (-3.14..3.14) I have to multiply the values by quite a big factor. As they have only 2 digits (0.00) of course that will increase the noise too.

But in principle, this is the way to go...

[CyberVillain]

Ok? But its unusable cause of the noise
Did you have any luck with the custom fusion if you use the Tablet? (Gyro required)

edit: Its in radians now so 3.14 = 180 degrees

[Parity]

Yes, unusable because of the noise.

Custom fusion? Not sure what you mean.
Do you mean android.Yaw and android.GoogleYaw ?

[CyberVillain]

Yes, unusable because of the noise.

Custom fusion? Not sure what you mean.
Do you mean android.Yaw and android.GoogleYaw ?

yeah, android.yaw uses a custom fusion that uses the raw values from the sensors (all 3 so wont work on your phone thats missing a gyroscope) and android.googleYaw uses the built in fusion on the phone, which have problems with noise (I guess you could filter it but its alot of noise, i'm not sure how that game have fixed it because it looks really nice in the vid)

[Parity]

I can code, but didnt work with smartphone so far.
But looking into your code I saw you use the Sensor.TYPE_ORIENTATION

Playing around a bit, I realized that this sensor does this strange 180° jump when device is close to vertical, while Sensor.TYPE_ROTATION_VECTOR does not change that way.

Seems to be a different interpretation of the sensor data. Tried playing around with that?