- send phones orientation using Madgwick AHRS

- data is transformed to json now when published

common/src/main/java/de/tudresden/inf/st/sensorsharing/common/MadgwickAHRS.java

+package de.tudresden.inf.st.sensorsharing.common;
+
+public class MadgwickAHRS {
+    /// <summary>
+    /// Gets or sets the sample period.
+    /// </summary>
+    public float SamplePeriod;
+
+    /// <summary>
+    /// Gets or sets the algorithm gain beta.
+    /// </summary>
+    public float Beta;
+
+    /// <summary>
+    /// Gets or sets the Quaternion output.
+    /// </summary>
+    public float[] Quaternion;
+
+    float roll;
+    float pitch;
+    float yaw;
+    boolean anglesComputed = false;
+
+    /// <summary>
+    /// Initializes a new instance of the <see cref="MadgwickAHRS"/> class.
+    /// </summary>
+    /// <param name="samplePeriod">
+    /// Sample period.
+    /// </param>
+    public MadgwickAHRS(float samplePeriod) {
+        this(samplePeriod, 1f);
+    }
+
+    /// <summary>
+    /// Initializes a new instance of the <see cref="MadgwickAHRS"/> class.
+    /// </summary>
+    /// <param name="samplePeriod">
+    /// Sample period.
+    /// </param>
+    /// <param name="beta">
+    /// Algorithm gain beta.
+    /// </param>
+    public MadgwickAHRS(float samplePeriod, float beta) {
+        SamplePeriod = samplePeriod;
+        Beta = beta;
+        Quaternion = new float[]{1f, 0f, 0f, 0f};
+    }
+
+    public void Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+        float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3];   // short name local variable for readability
+        float norm;
+        float hx, hy, _2bx, _2bz;
+        float s1, s2, s3, s4;
+        float qDot1, qDot2, qDot3, qDot4;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        float _2q1mx;
+        float _2q1my;
+        float _2q1mz;
+        float _2q2mx;
+        float _4bx;
+        float _4bz;
+        float _2q1 = 2f * q1;
+        float _2q2 = 2f * q2;
+        float _2q3 = 2f * q3;
+        float _2q4 = 2f * q4;
+        float _2q1q3 = 2f * q1 * q3;
+        float _2q3q4 = 2f * q3 * q4;
+        float q1q1 = q1 * q1;
+        float q1q2 = q1 * q2;
+        float q1q3 = q1 * q3;
+        float q1q4 = q1 * q4;
+        float q2q2 = q2 * q2;
+        float q2q3 = q2 * q3;
+        float q2q4 = q2 * q4;
+        float q3q3 = q3 * q3;
+        float q3q4 = q3 * q4;
+        float q4q4 = q4 * q4;
+
+        // Normalise accelerometer measurement
+        norm = (float) Math.sqrt(ax * ax + ay * ay + az * az);
+        if (norm == 0f) return; // handle NaN
+        norm = 1 / norm;        // use reciprocal for division
+        ax *= norm;
+        ay *= norm;
+        az *= norm;
+
+        // Normalise magnetometer measurement
+        norm = (float) Math.sqrt(mx * mx + my * my + mz * mz);
+        if (norm == 0f) return; // handle NaN
+        norm = 1 / norm;        // use reciprocal for division
+        mx *= norm;
+        my *= norm;
+        mz *= norm;
+
+        // Reference direction of Earth's magnetic field
+        _2q1mx = 2f * q1 * mx;
+        _2q1my = 2f * q1 * my;
+        _2q1mz = 2f * q1 * mz;
+        _2q2mx = 2f * q2 * mx;
+        hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
+        hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
+        _2bx = (float) Math.sqrt(hx * hx + hy * hy);
+        _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
+        _4bx = 2f * _2bx;
+        _4bz = 2f * _2bz;
+
+        // Gradient decent algorithm corrective step
+        s1 = -_2q3 * (2f * q2q4 - _2q1q3 - ax) + _2q2 * (2f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        s2 = _2q4 * (2f * q2q4 - _2q1q3 - ax) + _2q1 * (2f * q1q2 + _2q3q4 - ay) - 4f * q2 * (1 - 2f * q2q2 - 2f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        s3 = -_2q1 * (2f * q2q4 - _2q1q3 - ax) + _2q4 * (2f * q1q2 + _2q3q4 - ay) - 4f * q3 * (1 - 2f * q2q2 - 2f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        s4 = _2q2 * (2f * q2q4 - _2q1q3 - ax) + _2q3 * (2f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        norm = 1f / (float) Math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
+        s1 *= norm;
+        s2 *= norm;
+        s3 *= norm;
+        s4 *= norm;
+
+        // Compute rate of change of quaternion
+        qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;
+        qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;
+        qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;
+        qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;
+
+        // Integrate to yield quaternion
+        q1 += qDot1 * SamplePeriod;
+        q2 += qDot2 * SamplePeriod;
+        q3 += qDot3 * SamplePeriod;
+        q4 += qDot4 * SamplePeriod;
+        norm = 1f / (float) Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
+        Quaternion[0] = q1 * norm;
+        Quaternion[1] = q2 * norm;
+        Quaternion[2] = q3 * norm;
+        Quaternion[3] = q4 * norm;
+        anglesComputed = false;
+    }
+
+    public void Update(float gx, float gy, float gz, float ax, float ay, float az) {
+        float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3];   // short name local variable for readability
+        float norm;
+        float s1, s2, s3, s4;
+        float qDot1, qDot2, qDot3, qDot4;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        float _2q1 = 2f * q1;
+        float _2q2 = 2f * q2;
+        float _2q3 = 2f * q3;
+        float _2q4 = 2f * q4;
+        float _4q1 = 4f * q1;
+        float _4q2 = 4f * q2;
+        float _4q3 = 4f * q3;
+        float _8q2 = 8f * q2;
+        float _8q3 = 8f * q3;
+        float q1q1 = q1 * q1;
+        float q2q2 = q2 * q2;
+        float q3q3 = q3 * q3;
+        float q4q4 = q4 * q4;
+
+        // Normalise accelerometer measurement
+        norm = (float) Math.sqrt(ax * ax + ay * ay + az * az);
+        if (norm == 0f) return; // handle NaN
+        norm = 1 / norm;        // use reciprocal for division
+        ax *= norm;
+        ay *= norm;
+        az *= norm;
+
+        // Gradient decent algorithm corrective step
+        s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;
+        s2 = _4q2 * q4q4 - _2q4 * ax + 4f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
+        s3 = 4f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
+        s4 = 4f * q2q2 * q4 - _2q2 * ax + 4f * q3q3 * q4 - _2q3 * ay;
+        norm = 1f / (float) Math.sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
+        s1 *= norm;
+        s2 *= norm;
+        s3 *= norm;
+        s4 *= norm;
+
+        // Compute rate of change of quaternion
+        qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - Beta * s1;
+        qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - Beta * s2;
+        qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - Beta * s3;
+        qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - Beta * s4;
+
+        // Integrate to yield quaternion
+        q1 += qDot1 * SamplePeriod;
+        q2 += qDot2 * SamplePeriod;
+        q3 += qDot3 * SamplePeriod;
+        q4 += qDot4 * SamplePeriod;
+        norm = 1f / (float) Math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
+        Quaternion[0] = q1 * norm;
+        Quaternion[1] = q2 * norm;
+        Quaternion[2] = q3 * norm;
+        Quaternion[3] = q4 * norm;
+        anglesComputed = false;
+    }
+
+    public float getSamplePeriod() {
+        return SamplePeriod;
+    }
+
+    public void setSamplePeriod(float samplePeriod) {
+        SamplePeriod = samplePeriod;
+    }
+
+    public float getBeta() {
+        return Beta;
+    }
+
+    public void setBeta(float beta) {
+        Beta = beta;
+    }
+
+    public float[] getQuaternion() {
+        return Quaternion;
+    }
+
+    public void setQuaternion(float[] quaternion) {
+        Quaternion = quaternion;
+    }
+
+    public void computeAngles()
+    {
+        float q1 = Quaternion[0], q2 = Quaternion[1], q3 = Quaternion[2], q4 = Quaternion[3];   // short name local variable for readability
+        roll = (float) Math.atan2(q1*q2 + q3*q4, 0.5f - q2*q2 - q3*q3);
+        pitch = (float) Math.asin(-2.0f * (q2*q4 - q1*q3));
+        yaw = (float) Math.atan2(q2*q3 + q1*q4, 0.5f - q3*q3 - q4*q4);
+        anglesComputed = true;
+    }
+
+    public float getRoll() {
+        if (!anglesComputed) computeAngles();
+        return roll * 57.29578f;
+    }
+    public float getPitch() {
+        if (!anglesComputed) computeAngles();
+        return pitch * 57.29578f;
+    }
+    public float getYaw() {
+        if (!anglesComputed) computeAngles();
+        return yaw * 57.29578f + 180.0f;
+    }
+    public float getRollRadians() {
+        if (!anglesComputed) computeAngles();
+        return roll;
+    }
+    public float getPitchRadians() {
+        if (!anglesComputed) computeAngles();
+        return pitch;
+    }
+    public float getYawRadians() {
+        if (!anglesComputed) computeAngles();
+        return yaw;
+    }
+}

mobile/src/main/java/de/tudresden/inf/st/sensorsharing/MainActivity.java

@@ -19,6 +19,8 @@ import android.widget.EditText;
 import android.widget.TextView;
 import android.widget.Toast;
 
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
 import com.google.android.gms.wearable.MessageClient;
 import com.google.android.gms.wearable.MessageEvent;
 import com.google.android.gms.wearable.Wearable;
@@ -34,12 +36,13 @@ import org.eclipse.paho.client.mqttv3.MqttMessage;
 
 import java.nio.ByteBuffer;
 import java.text.DateFormat;
-import java.util.Arrays;
 import java.util.Date;
+import java.util.HashMap;
 import java.util.Locale;
 import java.util.Objects;
 import java.util.concurrent.ConcurrentHashMap;
 
+import de.tudresden.inf.st.sensorsharing.common.MadgwickAHRS;
 import de.tudresden.inf.st.sensorsharing.common.MqttUtils;
 import de.tudresden.inf.st.sensorsharing.common.Utils;
 import de.tudresden.inf.st.sensorsharing.settings.AppSettings;
@@ -52,6 +55,7 @@ import static de.tudresden.inf.st.sensorsharing.common.Constants.SUB_KEY_LINEAR_
 import static de.tudresden.inf.st.sensorsharing.common.Constants.SUB_KEY_NAME;
 import static de.tudresden.inf.st.sensorsharing.common.Constants.SUB_KEY_ROTATION_VECTOR;
 import static de.tudresden.inf.st.sensorsharing.settings.AppSettings.SETTINGS_MQTT_SERVER;
+import static java.lang.Math.atan2;
 
 public class MainActivity extends AppCompatActivity implements
         SensorEventListener,
@@ -64,6 +68,9 @@ public class MainActivity extends AppCompatActivity implements
     private SensorManager mSensorManager;
     private Sensor mLight;
     private Sensor mRotationVector;
+    private Sensor gyroSensor;
+    private Sensor accelSensor;
+    private Sensor magSensor;
 
     // mqtt management
     private static final int MQTT_DEFAULT_QOS = 0;
@@ -74,7 +81,9 @@ public class MainActivity extends AppCompatActivity implements
     private final ConcurrentHashMap<String, Boolean> sendMqttUpdates = new ConcurrentHashMap<>();
 
     // wearable management
-    /** Delay in milliseconds after which the wearable is assumed to be offline */
+    /**
+     * Delay in milliseconds after which the wearable is assumed to be offline
+     */
     public static final int DELAY_WEARABLE_ASSUMED_OFFLINE = 30000;
     private Date lastMessageFromWearable = new Date();
     private DateFormat dateTimeFormat = DateFormat.getDateTimeInstance(
@@ -82,9 +91,13 @@ public class MainActivity extends AppCompatActivity implements
 
     // sensor values of phone
     private float[] mRotationVectorValues;
+    private float[] gyroVectorValues;
+    private float[] accelVectorValues;
+    private float[] magVectorValues;
     private float mLightValue;
     private String clientId;
     private AppSettings settings;
+    MadgwickAHRS filter;
 
     private EditText valueServerURI;
 
@@ -97,6 +110,7 @@ public class MainActivity extends AppCompatActivity implements
 
         initMqttClientId();
         updateMqttTopics("unknown");
+        filter = new MadgwickAHRS(50, 1f);
 
         settings = new AppSettings(this);
         initViews();
@@ -108,7 +122,13 @@ public class MainActivity extends AppCompatActivity implements
             return;
         }
         mLight = mSensorManager.getDefaultSensor(Sensor.TYPE_LIGHT);
+
+        gyroSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
         mRotationVector = mSensorManager.getDefaultSensor(Sensor.TYPE_GAME_ROTATION_VECTOR);
+        magSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
+        if (mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER) != null){
+            accelSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
+        }
         if (mRotationVector == null) {
             mRotationVector = mSensorManager.getDefaultSensor(Sensor.TYPE_GEOMAGNETIC_ROTATION_VECTOR);
             if (mRotationVector == null) {
@@ -124,6 +144,9 @@ public class MainActivity extends AppCompatActivity implements
         // initialize sensor values
         mLightValue = -1;
         mRotationVectorValues = new float[3];
+        accelVectorValues = new float[3];
+        gyroVectorValues = new float[3];
+        magVectorValues = new float[3];
 
         updateWearStatus(false);
         connectMqtt(null);
@@ -170,7 +193,7 @@ public class MainActivity extends AppCompatActivity implements
         }
         wearTopics = new MqttTopics(subTopic);
         // use for phone model for now
-        phoneTopics = new MqttTopics(Build.DEVICE);
+        phoneTopics = new MqttTopics("phone");
     }
 
     private void setupSendUpdatesCheckbox() {
@@ -178,7 +201,7 @@ public class MainActivity extends AppCompatActivity implements
         final CheckBox sendUpdatesRotation = findViewById(R.id.checkBox_send_rotation_ahrs);
 
         sendMqttUpdates.put(phoneTopics.mqtt_topic_brightness, sendUpdates.isChecked());
-        sendMqttUpdates.put(phoneTopics.mqtt_topic_rotation, sendUpdatesRotation.isChecked());
+        sendMqttUpdates.put(phoneTopics.mqtt_topic_rotation_ahrs, sendUpdatesRotation.isChecked());
 
         CompoundButton.OnCheckedChangeListener onCheckedChangeListener = new CompoundButton.OnCheckedChangeListener() {
             @Override
@@ -188,7 +211,7 @@ public class MainActivity extends AppCompatActivity implements
                         sendMqttUpdates.put(phoneTopics.mqtt_topic_brightness, isChecked);
                         break;
                     case R.id.checkBox_send_rotation_ahrs:
-                        sendMqttUpdates.put(phoneTopics.mqtt_topic_rotation, sendUpdatesRotation.isChecked());
+                        sendMqttUpdates.put(phoneTopics.mqtt_topic_rotation_ahrs, sendUpdatesRotation.isChecked());
                         break;
                 }
             }
@@ -301,6 +324,15 @@ public class MainActivity extends AppCompatActivity implements
                 textView = findViewById(R.id.value_own_rotation);
                 textView.setText(Utils.formatArray3(mRotationVectorValues));
                 break;
+            case Sensor.TYPE_ACCELEROMETER:
+                accelVectorValues = sensorEvent.values;
+                break;
+            case Sensor.TYPE_GYROSCOPE:
+                gyroVectorValues = sensorEvent.values;
+                break;
+                case Sensor.TYPE_MAGNETIC_FIELD:
+                magVectorValues = sensorEvent.values;
+                break;
         }
         if (++sensorChangedCounter >= 15) {
             // only send every 15th change
@@ -320,6 +352,9 @@ public class MainActivity extends AppCompatActivity implements
         initMqttClientId();
         mSensorManager.registerListener(this, mLight, SensorManager.SENSOR_DELAY_UI);
         mSensorManager.registerListener(this, mRotationVector, SensorManager.SENSOR_DELAY_UI);
+        mSensorManager.registerListener(this, accelSensor, SensorManager.SENSOR_DELAY_UI);
+        mSensorManager.registerListener(this, gyroSensor, SensorManager.SENSOR_DELAY_UI);
+        mSensorManager.registerListener(this, magSensor, SensorManager.SENSOR_DELAY_UI);
         Wearable.getMessageClient(this).addListener(this);
     }
 
@@ -423,8 +458,35 @@ public class MainActivity extends AppCompatActivity implements
      * Whether the data is really published depends on the checkboxes.
      */
     private void sendUpdateOfSmartphone() {
-        sendUpdate(phoneTopics.mqtt_topic_brightness, Float.toString(mLightValue));
-        sendUpdate(phoneTopics.mqtt_topic_rotation, Arrays.toString(mRotationVectorValues));
+        ObjectMapper mapper = new ObjectMapper();
+
+        HashMap<String, Float> dataLight = new HashMap<>();
+        dataLight.put("brightness", mLightValue);
+        try {
+            String s = mapper.writeValueAsString(dataLight);
+            sendUpdate(phoneTopics.mqtt_topic_brightness, s);
+        } catch (JsonProcessingException e) {
+            e.printStackTrace();
+        }
+
+        filter.Update(gyroVectorValues[0], gyroVectorValues[1], gyroVectorValues[2],
+                accelVectorValues[0], accelVectorValues[1], accelVectorValues[2],
+                magVectorValues[0], magVectorValues[1], magVectorValues[2]);
+        filter.computeAngles();
+        float yaw = filter.getYaw();
+        float pitch = filter.getPitch();
+        float roll = filter.getRoll();
+        //System.out.println(String.format("%f, %f, %f", yaw, pitch, roll));
+        HashMap<String, Float> data = new HashMap<>();
+        data.put("heading", yaw);
+        data.put("pitch", pitch);
+        data.put("roll", roll);
+        try {
+            String s = mapper.writeValueAsString(data);
+            sendUpdate(phoneTopics.mqtt_topic_rotation_ahrs, s);
+        } catch (JsonProcessingException e) {
+            e.printStackTrace();
+        }
     }
 
     private void sendUpdate(String topic, String newValue) {